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

Control orientation

Browse Source
This commit is contained in:
Michel Breyer 2021-03-12 17:40:59 +01:00
parent e4822b0981
commit 1f839a5bb2
4 changed files with 125 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
controllers.py
View File

@ -2,19 +2,24 @@ import numpy as np
class CartesianPoseController:
def __init__(self, model):
def __init__(self, model, x0):
self.model = model
# self.x_d = x0
self.x_d = x0
self.kp = np.ones(6) * 5.0
def set_target(self, pose):
self.x_d = pose.translation
def set_target(self, x_d):
self.x_d = x_d
def update(self, q, dq):
t = self.model.pose(q).translation
J = self.model.jacobian(q)[:3, :]
J_pinv = np.linalg.pinv(J)
x = self.model.pose(q)
x_d = self.x_d
err = 2.0 * (self.x_d - t)
cmd = np.dot(J_pinv, err)
err = np.zeros(6)
err[:3] = x_d.translation - x.translation
err[3:] = (x_d.rotation * x.rotation.inv()).as_rotvec()
J = self.model.jacobian(q)
J_pinv = np.linalg.pinv(J)
cmd = np.dot(J_pinv, self.kp * err)
return cmd

14
robot_sim.py
View File

@ -11,15 +11,13 @@ from utils import *
class PandaArm(object):
def __init__(self):
self.num_dof = 7
self.ee_link_id = 7
self.ee_frame_id = "panda_link7"
self.nr_dof = 7
self.named_joint_values = {"ready": [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]}
self.upper_limits = [-7] * self.num_dof
self.lower_limits = [7] * self.num_dof
self.ranges = [7] * self.num_dof
self.upper_limits = [-7] * self.nr_dof
self.lower_limits = [7] * self.nr_dof
self.ranges = [7] * self.nr_dof
def load(self, pose):
self.T_world_base = pose
@ -29,8 +27,8 @@ class PandaArm(object):
baseOrientation=pose.rotation.as_quat(),
useFixedBase=True,
)
for i in range(self.num_dof):
p.resetJointState(self.uid, i, self.named_joint_values["ready"][i])
for i, q_i in enumerate(self.named_joint_values["ready"]):
p.resetJointState(self.uid, i, q_i)
def get_state(self):
joint_states = p.getJointStates(self.uid, range(p.getNumJoints(self.uid)))[:7]

7
run_demo.py
View File

@ -14,10 +14,13 @@ rospy.init_node("demo")
env = SimPandaEnv(gui)
model = Model("panda_link0", "panda_link8")
controller = CartesianPoseController(model)
q, dq = env.arm.get_state()
marker = InteractiveMarkerWrapper("target", "panda_link0", model.pose(q))
x0 = model.pose(q)
controller = CartesianPoseController(model, x0)
marker = InteractiveMarkerWrapper("target", "panda_link0", x0)
# run the control loop
while True:

117
utils.py
View File

@ -2,22 +2,23 @@ import numpy as np
from scipy.spatial.transform import Rotation
import PyKDL as kdl
import geometry_msgs.msg
from interactive_markers.interactive_marker_server import *
from interactive_markers.menu_handler import *
from visualization_msgs.msg import *
class InteractiveMarkerWrapper(object):
def __init__(self, name, frame_id, pose):
def __init__(self, name, frame_id, x0):
self.pose = x0
server = InteractiveMarkerServer(topic_ns=name)
target_marker = InteractiveMarker()
target_marker.header.frame_id = frame_id
target_marker.name = name
target_marker.scale = 0.2
target_marker.pose.position.x = pose.translation[0]
target_marker.pose.position.y = pose.translation[1]
target_marker.pose.position.z = pose.translation[2]
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame_id
int_marker.name = name
int_marker.scale = 0.2
int_marker.pose = to_pose_msg(x0)
marker = Marker()
marker.type = Marker.SPHERE
@ -28,40 +29,65 @@ class InteractiveMarkerWrapper(object):
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.6
ctrl = InteractiveMarkerControl()
ctrl.always_visible = True
ctrl.markers.append(marker)
target_marker.controls.append(ctrl)
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.orientation.w = 1
ctrl.orientation.x = 1
ctrl.orientation.y = 0
ctrl.orientation.z = 0
ctrl.name = "rotate_x"
ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.orientation.w = 1
ctrl.orientation.x = 0
ctrl.orientation.y = 0
ctrl.orientation.z = 1
ctrl.name = "rotate_y"
ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.orientation.w = 1
ctrl.orientation.x = 0
ctrl.orientation.y = 1
ctrl.orientation.z = 0
ctrl.name = "rotate_z"
ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.name = "move_x"
ctrl.orientation.w = 1.0
ctrl.orientation.x = 1.0
ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
target_marker.controls.append(ctrl)
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.name = "move_y"
ctrl.orientation.w = 1.0
ctrl.orientation.y = 1.0
ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
target_marker.controls.append(ctrl)
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.name = "move_z"
ctrl.orientation.w = 1.0
ctrl.orientation.z = 1.0
ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
target_marker.controls.append(ctrl)
int_marker.controls.append(ctrl)
server.insert(target_marker, self.cb)
server.insert(int_marker, self.cb)
server.applyChanges()
self.pose = pose
def cb(self, feedback):
pos = feedback.pose.position
self.pose = Transform(Rotation.identity(), np.r_[pos.x, pos.y, pos.z])
self.pose = from_pose_msg(feedback.pose)
def get_pose(self):
return self.pose
@ -126,6 +152,9 @@ class Transform(object):
return cls(rotation, translation)
# KDL Conversions
def to_kdl_jnt_array(q):
jnt_array = kdl.JntArray(len(q))
for i, q_i in enumerate(q):
@ -139,3 +168,57 @@ def kdl_to_mat(m):
for j in range(m.columns()):
mat[i, j] = m[i, j]
return mat
# ROS Conversions
def to_point_msg(point):
msg = geometry_msgs.msg.Point()
msg.x = point[0]
msg.y = point[1]
msg.z = point[2]
return msg
def from_point_msg(msg):
return np.r_[msg.x, msg.y, msg.z]
def to_vector3_msg(vector3):
msg = geometry_msgs.msg.Vector3()
msg.x = vector3[0]
msg.y = vector3[1]
msg.z = vector3[2]
return msg
def from_vector3_msg(msg):
return np.r_[msg.x, msg.y, msg.z]
def to_quat_msg(orientation):
quat = orientation.as_quat()
msg = geometry_msgs.msg.Quaternion()
msg.x = quat[0]
msg.y = quat[1]
msg.z = quat[2]
msg.w = quat[3]
return msg
def from_quat_msg(msg):
return Rotation.from_quat([msg.x, msg.y, msg.z, msg.w])
def to_pose_msg(transform):
msg = geometry_msgs.msg.Pose()
msg.position = to_point_msg(transform.translation)
msg.orientation = to_quat_msg(transform.rotation)
return msg
def from_pose_msg(msg):
position = from_point_msg(msg.position)
orientation = from_quat_msg(msg.orientation)
return Transform(orientation, position)