hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Small change to the controller

Browse Source
This commit is contained in:
Michel Breyer 2021-03-18 10:10:56 +01:00
parent 30f9934154
commit 622665cbc9
3 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
controllers.py
View File

@ -2,12 +2,12 @@ import numpy as np
class CartesianPoseController: class CartesianPoseController:
def __init__(self, robot, model, rate): def __init__(self, robot, model, x0, rate):
self.robot = robot self.robot = robot
self.model = model self.model = model
self.rate = rate self.rate = rate
self.x_d = None self.x_d = x0
self.kp = np.ones(6) * 5.0 self.kp = np.ones(6) * 4.0
def set_target(self, pose): def set_target(self, pose):
self.x_d = pose self.x_d = pose

4
run_demo.py
View File

@ -10,12 +10,14 @@ gui = True
rospy.init_node("demo") rospy.init_node("demo")
env = SimPandaEnv(gui) env = SimPandaEnv(gui)
env.controller = CartesianPoseController(env.arm, env.model, 60)
q, dq = env.arm.get_state() q, dq = env.arm.get_state()
x0 = env.model.pose(q) x0 = env.model.pose(q)
env.controller = CartesianPoseController(env.arm, env.model, x0, 60)
marker = InteractiveMarkerWrapper("target", "panda_link0", x0) marker = InteractiveMarkerWrapper("target", "panda_link0", x0)
while True: while True:
env.controller.set_target(marker.pose) env.controller.set_target(marker.pose)
env.camera.update()
env.forward(0.1) env.forward(0.1)

3
utils.py
View File

@ -79,9 +79,6 @@ class InteractiveMarkerWrapper(object):
class Transform(object): class Transform(object):
def __init__(self, rotation, translation): def __init__(self, rotation, translation):
assert isinstance(rotation, Rotation)
assert isinstance(translation, (np.ndarray, list))
self.rotation = rotation self.rotation = rotation
self.translation = np.asarray(translation, np.double) self.translation = np.asarray(translation, np.double)