def scale_trajectory_speed(traj, scale):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the planned trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and scale the time from start, speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajectory
return new_traj
def main():
pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory)
rospy.init_node('traj_test')
p1 = JointTrajectoryPoint()
p1.positions = [0,0,0,0,0,0,0]
p1.velocities = [0,0,0,0,0,0,0]
p1.accelerations = [0,0,0,0,0,0,0]
p2 = JointTrajectoryPoint()
p2.positions = [0,0,1.0,0,-1.0,0,0]
p2.velocities = [0,0,0,0,0,0,0]
p2.accelerations = [0,0,0,0,0,0,0]
p2.time_from_start = rospy.Time(4.0)
p3 = JointTrajectoryPoint()
p3.positions = [0,0,-1.0,0,1.0,0,0]
p3.velocities = [0,0,0,0,0,0,0]
p3.accelerations = [0,0,0,0,0,0,0]
p3.time_from_start = rospy.Time(20.0)
traj = JointTrajectory()
traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7']
traj.points = [p1,p2,p3]
rospy.loginfo(traj)
r = rospy.Rate(1) # 10hz
pub.publish(traj)
r.sleep()
pub.publish(traj)
def readInPoses(self):
poses = rospy.get_param('~poses')
rospy.loginfo("Found %d poses on the param server", len(poses))
for key,value in poses.iteritems():
try:
# TODO: handle multiple points in trajectory
trajectory = JointTrajectory()
trajectory.joint_names = value["joint_names"]
point = JointTrajectoryPoint()
point.time_from_start = Duration(value["time_from_start"])
point.positions = value["positions"]
trajectory.points = [point]
self.poseLibrary[key] = trajectory
except:
rospy.logwarn("Could not parse pose \"%s\" from the param server:", key);
rospy.logwarn(sys.exc_info())
# add a default crouching pose:
if not "crouch" in self.poseLibrary:
trajectory = JointTrajectory()
trajectory.joint_names = ["Body"]
point = JointTrajectoryPoint()
point.time_from_start = Duration(1.5)
point.positions = [0.0,0.0, # head
1.545, 0.33, -1.57, -0.486, 0.0, 0.0, # left arm
-0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # left leg
-0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # right leg
1.545, -0.33, 1.57, 0.486, 0.0, 0.0] # right arm
trajectory.points = [point]
self.poseLibrary["crouch"] = trajectory;
def tuck(self):
# prepare a joint trajectory
msg = JointTrajectory()
msg.joint_names = servos
msg.points = list()
point = JointTrajectoryPoint()
point.positions = forward
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(5.0)
msg.points.append(point)
point = JointTrajectoryPoint()
point.positions = to_side
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(8.0)
msg.points.append(point)
point = JointTrajectoryPoint()
point.positions = tucked
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(11.0)
msg.points.append(point)
# call action
msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
goal = FollowJointTrajectoryGoal()
goal.trajectory = msg
self._client.send_goal(goal)
def dual_arm_test():
pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory)
cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command', JointTrajectory)
rospy.init_node('dual_arm_test')
rospy.sleep(1.0)
while not rospy.is_shutdown():
traj = JointTrajectory()
traj.joint_names = ['lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint', 'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint']
ptn = JointTrajectoryPoint()
ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3]
ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.points = []
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
def get_grasp_posture(pre):
pre_grasp_posture = JointTrajectory()
pre_grasp_posture.header.frame_id = END_EFFECTOR
pre_grasp_posture.header.stamp = rospy.Time.now()
pre_grasp_posture.joint_names = [GRIPPER_JOINTS]
pos = JointTrajectoryPoint()
if pre: # gripper open
pos.positions = [0.0]
else: # gripper closed
pos.positions = [0.04]
pre_grasp_posture.points.append(pos)
return pre_grasp_posture
def send_cmd_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
# Check if we're dealing with one single float value
if len(self.joint_names) > 1:
jtp.positions = self.position
else:
jtp.positions = [self.position]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(1.0)
jtm.points = [jtp]
return jtm
def move_torso(self, string_operation):
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
if string_operation == "lift":
jtp.positions = [0.34]
elif string_operation == "lower":
jtp.positions = [0.15]
else:
return
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
rospy.loginfo("Moving torso " + string_operation)
self.torso_cmd.publish(jt)
def __init__(self, test="idle", output="screen", ):
if output == "file":
try:
os.mkdir(os.path.expanduser("~/cwru_battery_analyzer/"))
except Exception:
pass
dateTime = datetime.now().strftime("%Y-%m-%d_%H%M%S")
self.file_handle = open(os.path.expanduser("~/cwru_battery_analyzer/"+dateTime+".csv"), "w")
self.file_handle.write('Time,Battery Voltage,13.8v Rail Voltage,cRIO Voltage\n')
self.file_output = True
else:
self.file_output = False
if test == "idle":
self.behavior = self.idle
elif test == "drivetrain":
self.commandPublisher = rospy.Publisher("cmd_vel", Twist)
self.twistMsg = Twist()
self.twistMsg.angular.x = 0.0
self.twistMsg.angular.y = 0.0
self.twistMsg.angular.z = 0.0
self.twistMsg.linear.x = 0.0
self.twistMsg.linear.y = 0.0
self.twistMsg.linear.z = 0.0
self.twistIncrement = 0.1
self.behavior = self.exerciseDT
rospy.on_shutdown(self.stopDT)
elif test == "arm":
self.armPublisher = rospy.Publisher("command", JointTrajectory)
self.gripperService = rospy.ServiceProxy('/abby_gripper/gripper', gripper)
self.gripperStatus = True
self.jointTrajectoryMsg = JointTrajectory()
self.jointTrajectoryMsg.joint_names= ["joint1","joint2","joint3","joint4","joint5","joint6"]
jointAngles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #initial arm position in radians
jointRange = 2*.77777770 #radians
jointNum = 0
for seq in range(0,10):
jointAngles[jointNum] = jointAngles[jointNum] + (jointRange/11)
jointTrajectoryPt = JointTrajectoryPoint()
jointTrajectoryPt.positions = copy.deepcopy(jointAngles)
self.jointTrajectoryMsg.points.append(jointTrajectoryPt)
for seq in range(10,0):
jointAngles[jointNum] = jointAngles[jointNum] + (jointRange/11)
jointTrajectoryPt = JointTrajectoryPoint()
jointTrajectoryPt.positions = copy.deepcopy(jointAngles)
self.jointTrajectoryMsg.points.append(jointTrajectoryPt)
self.behavior = self.exerciseArm
self.batteryListener = rospy.Subscriber("power_state", PowerState, self.batteryCallback)
def check_collision(self):
if not self.collsion_srv:
return False
# Create a new trajectory by combining both the trajectories, assumption is that the timing is same
# The order of joining should be [left + right]
self.adjust_traj_length()
traj = JointTrajectory()
traj.joint_names = self.left._goal.trajectory.joint_names + self.right._goal.trajectory.joint_names
no_points = len(self.left._goal.trajectory.points)
for index in xrange(no_points):
pt = JointTrajectoryPoint()
left_pt = self.left._goal.trajectory.points[index]
right_pt = self.right._goal.trajectory.points[index]
pt.positions = left_pt.positions + right_pt.positions
pt.velocities = left_pt.velocities + right_pt.velocities
pt.time_from_start = left_pt.time_from_start
traj.points.append(pt)
msg = CollisionCheckRequest()
msg.trajectory = traj
try:
collision_service = rospy.ServiceProxy("collision_check", CollisionCheck)
resp = collision_service(msg)
if resp.collision_found:
rospy.logwarn("Collision Found")
else:
rospy.loginfo("Collision not found, good to go :)")
return resp.collision_found
except rospy.ServiceException as e:
rospy.logwarn("Exception on collision check {}".format(e))
return True
def build_traj(start, end, duration):
if sorted(start.name) <> sorted(end.name):
rospy.logerr('Start and End position joint_names mismatch')
raise
# assume the start-position joint-ordering
joint_names = start.name
# 始点定義
start_pt = JointTrajectoryPoint()
start_pt.positions = start.position
start_pt.velocities = [0]*len(start.position)
start_pt.time_from_start = rospy.Duration(0.0) # 始点なので、待ち時間 = 0 [sec]
# 中間地点定義
middle_pt = JointTrajectoryPoint()
for j in joint_names:
idx = end.name.index(j)
middle_pt.positions.append((start.position[idx] + end.position[idx])/2.0)
middle_pt.velocities.append(0)
middle_pt.time_from_start = rospy.Duration(duration) # 中間地点までの到達時間 = duration [sec]
# 執着地点
end_pt = JointTrajectoryPoint()
for j in joint_names:
idx = end.name.index(j)
end_pt.positions.append(end.position[idx])
end_pt.velocities.append(0.0)
end_pt.time_from_start = rospy.Duration(duration*2) # 終点までの到達時間 = duration*2 [sec]
return JointTrajectory(joint_names=joint_names, points=[start_pt, middle_pt, end_pt])
def build_traj(start, end, duration):
# print start.name
# print end.name
if sorted(start.name) <> sorted(end.name):
rospy.logerr('Start and End position joint_names mismatch')
raise
# assume the start-position joint-ordering
joint_names = start.name
start_pt = JointTrajectoryPoint()
start_pt.positions = start.position
start_pt.velocities = [0.05]*len(start.position)
start_pt.time_from_start = rospy.Duration(0.0)
end_pt = JointTrajectoryPoint()
# end_pt = start_pt;
# end_pt.positions[3] = end_pt.positions[3]+.1
for j in joint_names:
idx = end.name.index(j)
end_pt.positions.append(end.position[idx]) # reorder to match start-pos joint ordering
end_pt.velocities.append(0)
end_pt.time_from_start = rospy.Duration(duration)
return JointTrajectory(joint_names=joint_names, points=[start_pt, end_pt])
def move(self, angles):
goal = JointTrajectoryGoal()
char = self.name[0] #either 'r' or 'l'
goal.trajectory.joint_names = [char+'_shoulder_pan_joint',
char+'_shoulder_lift_joint',
char+'_upper_arm_roll_joint',
char+'_elbow_flex_joint',
char+'_forearm_roll_joint',
char+'_wrist_flex_joint',
char+'_wrist_roll_joint']
point = JointTrajectoryPoint()
new_angles = list(angles)
#replace angles that have None with last commanded joint position from this Arm class.
for i,ang in enumerate(angles):
if ang is None:
assert self.last_angles is not None
new_angles[i] = self.last_angles[i]
self.last_angles = new_angles
point.positions = new_angles
point.time_from_start = rospy.Duration(3)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def set_jep(self, arm, q, duration=0.15):
if q is None or len(q) != 7:
raise RuntimeError("set_jep value is " + str(q))
self.arm_state_lock[arm].acquire()
jtg = JointTrajectoryGoal()
jtg.trajectory.joint_names = self.joint_names_list[arm]
jtp = JointTrajectoryPoint()
jtp.positions = q
#jtp.velocities = [0 for i in range(len(q))]
#jtp.accelerations = [0 for i in range(len(q))]
jtp.time_from_start = rospy.Duration(duration)
jtg.trajectory.points.append(jtp)
self.joint_action_client[arm].send_goal(jtg)
self.jep[arm] = q
cep, r = self.arms.FK_all(arm, q)
self.arm_state_lock[arm].release()
o = np.matrix([0.,0.,0.,1.]).T
cep_marker = hv.single_marker(cep, o, 'sphere',
'/torso_lift_link', color=(0., 0., 1., 1.),
scale = (0.02, 0.02, 0.02),
m_id = self.cep_marker_id)
cep_marker.header.stamp = rospy.Time.now()
self.marker_pub.publish(cep_marker)
def execute(self, userdata):
rospy.loginfo('In create_move_group_joints_goal')
_move_joint_goal = FollowJointTrajectoryGoal()
_move_joint_goal.trajectory.joint_names = userdata.move_joint_list
jointTrajectoryPoint = JointTrajectoryPoint()
jointTrajectoryPoint.positions = userdata.move_joint_poses
for x in range(0, len(userdata.move_joint_poses)):
jointTrajectoryPoint.velocities.append(0.0)
jointTrajectoryPoint.time_from_start = rospy.Duration(2.0)
_move_joint_goal.trajectory.points.append(jointTrajectoryPoint)
for joint in _move_joint_goal.trajectory.joint_names:
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
_move_joint_goal.goal_tolerance.append(goal_tol)
_move_joint_goal.goal_time_tolerance = rospy.Duration(2.0)
_move_joint_goal.trajectory.header.stamp = rospy.Time.now()
rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal))
userdata.move_joint_goal = _move_joint_goal
return 'succeeded'
def test_joint_trajectory_action(self):
larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
larm.wait_for_server()
try:
self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
except tf.Exception:
self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
(trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
goal = JointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.joint_names.append("J1")
goal.trajectory.joint_names.append("J2")
goal.trajectory.joint_names.append("J3")
goal.trajectory.joint_names.append("J4")
goal.trajectory.joint_names.append("J5")
goal.trajectory.joint_names.append("J6")
goal.trajectory.joint_names.append("J7")
point = JointTrajectoryPoint()
point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
point.time_from_start = rospy.Duration(5.0)
goal.trajectory.points.append(point)
larm.send_goal(goal)
larm.wait_for_result()
(trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
def control_loop(self):
if self.commands is None:
raise Exception('Command list is empty. Was the control plan loaded?')
# loop through the command list
for cmd in self.commands:
# wait for proxy to be in active state
self.wait_for_state(self._proxy_state_running)
# process commands
cmd_spec_str = None
spec = None
t = cmd[ProxyCommand.key_command_type]
if not (t == "noop"):
cmd_spec_str = cmd[ProxyCommand.key_command_spec]
if not isinstance(cmd_spec_str, basestring):
spec = float(cmd_spec_str)
else:
spec = self.positions[cmd_spec_str]
rospy.loginfo("Command type: " + t + ", spec: " + str(cmd_spec_str) + ", value: " + str(spec))
# check for any wait depends
self.wait_for_depend(cmd)
# execute command
# could do this with a dictionary-based function lookup, but who cares
if t == 'noop':
rospy.loginfo("Command type: noop")
self.wait_for_depend(cmd)
elif t == 'sleep':
rospy.loginfo("sleep command")
v = float(spec)
rospy.sleep(v)
elif t == 'move_gripper':
rospy.loginfo("gripper command")
self.move_gripper(spec)
elif t == 'move_arm':
rospy.loginfo("move_arm command")
rospy.logdebug(spec)
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = self.arm_joint_names
jtp = JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(0.5) # fudge factor for gazebo controller
jtp.positions = spec
jtp.velocities = [0]*len(spec)
goal.trajectory.points.append(jtp)
self._arm_goal = copy.deepcopy(goal)
self.move_arm()
elif t == 'plan_exec_arm':
rospy.loginfo("plan and execute command not implemented")
raise NotImplementedError()
else:
raise Exception("Invalid command type: " + str(cmd.type))
# check for any set dependencies action
self.set_depend(cmd)
# check for any clear dependencies action
self.clear_depend(cmd)
def __init__(self, arm = 'l', record_data = False):
self.arm = arm
if arm == 'r':
print "using right arm on Darci"
self.RIGHT_ARM = 0 # i don't think this was used anywhere
else:
self.LEFT_ARM = 1
# print "Left and both arms not implemented in this client yet ... \n"
# print "will require writing different function calls since joint data for left and right arm come in together from the meka server"
# sys.exit()
self.kinematics = dak.DarciArmKinematics(arm)
self.joint_pub = rospy.Publisher('/'+self.arm+'_arm_controller/command', JointTrajectory)
self.joint_names = None
self.lock = RLock()
self.joint_angles = None
self.joint_velocities = None
self.J_h = None
self.time = None
self.desired_joint_angles = None
self.stiffness_percent = 0.75
self.ee_force = None
self.ee_torque = None
self.skins = None #will need code for all of these skin interfaces
self.Jc_l = []
self.n_l = []
self.values_l = []
#values from m3gtt repository in robot_config folder
# These could be read in from a yaml file like this (import yaml; stream = open("FILE_NAME_HERE", 'r'); data = yaml.load(stream))
# However, not clear if absolute path to yaml file (possibly on another computer) is better then just defining it here
# The downside is obviously if someone tunes gains differently on the robot.
self.joint_stiffness = (np.array([1, 1, 1, 1, 0.06, 0.08, 0.08])*180/np.pi*self.stiffness_percent).tolist()
self.joint_damping = (np.array([0.06, 0.1, 0.015, 0.015, 0.0015, 0.002, 0.002])*180/np.pi*self.stiffness_percent).tolist()
self.record_data = record_data
if self.record_data:
from collections import deque
self.q_record = deque()
self.qd_record = deque()
self.times = deque()
self.state_sub = rospy.Subscriber('/'+self.arm+'_arm_controller/state', JointTrajectoryControllerState, self.robotStateCallback)
rospy.sleep(1.0)
while self.joint_angles is None:
rospy.sleep(0.05)
self.desired_joint_angles = copy.copy(self.joint_angles)
self.joint_cmd = JointTrajectory()
self.joint_cmd.header.stamp = rospy.Time.now()
self.joint_cmd.header.frame_id = '/torso_lift_link'
self.joint_cmd.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = self.desired_joint_angles
jtp.velocities = [1.]*len(self.joint_names)
self.joint_cmd.points = [jtp]
self.joint_pub.publish(self.joint_cmd)
def get_place_locations(self):
locations = []
for xx in np.arange(0.4, 0.7, 0.1):
for yy in np.arange(0.5, 0.9, 0.1):
pl = PlaceLocation()
pt = JointTrajectoryPoint()
pt.positions = [0.5]
pl.post_place_posture.points.append(pt)
pl.post_place_posture.joint_names = ['l_gripper_motor_screw_joint']
pl.place_pose.header.frame_id = "odom_combined"
pl.place_pose.header.stamp = rospy.Time.now()
pl.place_pose.pose.position.x = xx
pl.place_pose.pose.position.y = yy
pl.place_pose.pose.position.z = 1.05
# Quaternion obtained by pointing wrist down
#pl.place_pose.pose.orientation.w = 1.0
pl.place_pose.pose.orientation.x = 0.761
pl.place_pose.pose.orientation.y = -0.003
pl.place_pose.pose.orientation.z = -0.648
pl.place_pose.pose.orientation.w = 0.015
pl.pre_place_approach.direction.header.frame_id = "odom_combined"
pl.pre_place_approach.direction.vector.z = -1.0
pl.pre_place_approach.desired_distance = 0.2
pl.pre_place_approach.min_distance = 0.1
pl.post_place_retreat.direction.header.frame_id = "odom_combined"
pl.post_place_retreat.direction.vector.z = 1.0
pl.post_place_retreat.desired_distance = 0.2
pl.post_place_retreat.min_distance = 0.1
locations.append(pl)
return locations
def create_placings_from_object_pose(self, posestamped):
""" Create a list of PlaceLocation of the object rotated every 15deg"""
place_locs = []
pre_grasp_posture = JointTrajectory()
# Actually ignored....
pre_grasp_posture.header.frame_id = self._grasp_pose_frame_id
pre_grasp_posture.joint_names = [
name for name in self._gripper_joint_names.split()]
jtpoint = JointTrajectoryPoint()
jtpoint.positions = [
float(pos) for pos in self._gripper_pre_grasp_positions.split()]
jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
pre_grasp_posture.points.append(jtpoint)
# Generate all the orientations every step_degrees_yaw deg
for yaw_angle in np.arange(0.0, 2.0 * pi, radians(self._step_degrees_yaw)):
pl = PlaceLocation()
pl.place_pose = posestamped
newquat = quaternion_from_euler(0.0, 0.0, yaw_angle)
pl.place_pose.pose.orientation = Quaternion(
newquat[0], newquat[1], newquat[2], newquat[3])
# TODO: the frame is ignored, this will always be the frame of the gripper
# so arm_tool_link
pl.pre_place_approach = self.createGripperTranslation(
Vector3(1.0, 0.0, 0.0))
pl.post_place_retreat = self.createGripperTranslation(
Vector3(-1.0, 0.0, 0.0))
pl.post_place_posture = pre_grasp_posture
place_locs.append(pl)
return place_locs
def _store_path(self, final_path, retrieved_path):
store_request = ManagePathLibraryRequest()
store_request.joint_names = self.current_joint_names
store_request.robot_name = self.robot_name
store_request.action = store_request.ACTION_STORE
# convert into apporpriate request.
for point in final_path:
jtp = JointTrajectoryPoint()
jtp.positions = point
store_request.path_to_store.append(jtp)
for point in retrieved_path:
jtp = JointTrajectoryPoint()
jtp.positions = point
store_request.retrieved_path.append(jtp)
store_response = self.manage_library_client(store_request)
return (store_response.path_stored, store_response.num_library_paths)
def stage_3(self):
# Move arm to grasping position
rospy.loginfo("Begin stage 3")
# Create a joint state publisher
self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory)
rospy.sleep(1)
# How fast will we update the robot's movement? in Hz
rate = 10
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Define a joint trajectory message for grasp position
rospy.loginfo("Position arm in grasp position")
msg = JointTrajectory()
msg.header.seq = 1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '0'
msg.joint_names.append('joint_1')
msg.joint_names.append('joint_2')
msg.joint_names.append('joint_3')
msg.joint_names.append('joint_4')
msg.joint_names.append('joint_5')
# Define joint trajectory points
point = JointTrajectoryPoint()
point.positions = [0.0, 0.0, 0.0, 1.6, 0.1]
point.velocities = [20.0, 20.0, 20.0, 10.0, 10.0]
point.time_from_start = rospy.Duration(3.0)
msg.points.append(point)
# publish joint stage message
self.arm_pub.publish(msg)
rospy.sleep(3)
def states_to_trajectory(states, time_step=0.1):
"""
Converts a list of RobotState/JointState in Robot Trajectory
:param robot_states: list of RobotState
:param time_step: duration in seconds between two consecutive points
:return: a Robot Trajectory
"""
rt = RobotTrajectory()
for state_idx, state in enumerate(states):
if isinstance(state, RobotState):
state = state.joint_state
jtp = JointTrajectoryPoint()
jtp.positions = state.position
# jtp.velocities = state.velocity
# Probably does not make sense to keep velocities and efforts here
# jtp.effort = state.effort
jtp.time_from_start = rospy.Duration(state_idx*time_step)
rt.joint_trajectory.points.append(jtp)
if len(states) > 0:
if isinstance(states[0], RobotState):
rt.joint_trajectory.joint_names = states[0].joint_state.name
else:
rt.joint_trajectory.joint_names = states[0].joint_names
return rt
def test_trajectory(self):
# our goal variable
goal = FollowJointTrajectoryGoal()
# First, the joint names, which apply to all waypoints
goal.trajectory.joint_names = ('shoulder_pitch_joint',
'shoulder_pan_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_pitch_joint',
'wrist_roll_joint')
# We will have two waypoints in this goal trajectory
# First trajectory point
# To be reached 1 second after starting along the trajectory
point = JointTrajectoryPoint()
point.positions = [0.0] * 7
point.velocities = [1.0] * 7
point.time_from_start = rospy.Duration(3.0)
goal.trajectory.points.append(point)
#we are done; return the goal
return goal
def test_joint_angles(self):
larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
larm.wait_for_server()
try:
self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
except tf.Exception:
self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")
(trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
goal = JointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.joint_names.append("LARM_SHOULDER_P")
goal.trajectory.joint_names.append("LARM_SHOULDER_R")
goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
goal.trajectory.joint_names.append("LARM_ELBOW")
goal.trajectory.joint_names.append("LARM_WRIST_Y")
goal.trajectory.joint_names.append("LARM_WRIST_P")
point = JointTrajectoryPoint()
point.positions = [ x * math.pi / 180.0 for x in [30,30,30,-90,-40,-30] ]
point.time_from_start = rospy.Duration(5.0)
goal.trajectory.points.append(point)
larm.send_goal(goal)
larm.wait_for_result()
(trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
def goal_cb(self, pt_msg):
rospy.loginfo("[{0}] New LookatIK goal received.".format(rospy.get_name()))
if (pt_msg.header.frame_id != self.camera_ik.base_frame):
try:
now = pt_msg.header.stamp
self.tfl.waitForTransform(pt_msg.header.frame_id,
self.camera_ik.base_frame,
now, rospy.Duration(1.0))
pt_msg = self.tfl.transformPoint(self.camera_ik.base_frame, pt_msg)
except (LookupException, ConnectivityException, ExtrapolationException):
rospy.logwarn("[{0}] TF Error tranforming point from {1} to {2}".format(rospy.get_name(),
pt_msg.header.frame_id,
self.camera_ik.base_frame))
target = np.array([pt_msg.point.x, pt_msg.point.y, pt_msg.point.z])
# Get IK Solution
current_angles = list(copy.copy(self.joint_angles_act))
iksol = self.camera_ik.lookat_ik(target, current_angles[:len(self.camera_ik.arm_indices)])
# Start with current angles, then replace angles in camera IK with IK solution
# Use desired joint angles to avoid sagging over time
jtp = JointTrajectoryPoint()
jtp.positions = list(copy.copy(self.joint_angles_des))
for i, joint_name in enumerate(self.camera_ik.arm_joint_names):
jtp.positions[self.joint_names.index(joint_name)] = iksol[i]
deltas = np.abs(np.subtract(jtp.positions, current_angles))
duration = np.max(np.divide(deltas, self.max_vel_rot))
jtp.time_from_start = rospy.Duration(duration)
jt = JointTrajectory()
jt.joint_names = self.joint_names
jt.points.append(jtp)
rospy.loginfo("[{0}] Sending Joint Angles.".format(rospy.get_name()))
self.joint_traj_pub.publish(jt)
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
if self._mode == "velocity":
velocities = [0.0] * len(joint_names)
cmd = dict(zip(joint_names, velocities))
while not self._server.is_new_goal_available() and self._alive:
self._limb.set_joint_velocities(cmd)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
elif self._mode == "position" or self._mode == "position_w_id":
raw_pos_mode = self._mode == "position_w_id"
if raw_pos_mode:
pnt = JointTrajectoryPoint()
pnt.positions = self._get_current_position(joint_names)
if dimensions_dict["velocities"]:
pnt.velocities = [0.0] * len(joint_names)
if dimensions_dict["accelerations"]:
pnt.accelerations = [0.0] * len(joint_names)
while not self._server.is_new_goal_available() and self._alive:
self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
# zero inverse dynamics feedforward command
if self._mode == "position_w_id":
pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
self._pub_ff_cmd.publish(ff_pnt)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
def traj_speed_up(traj, spd=3.0):
new_traj = RobotTrajectory()
new_traj = traj
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * spd
point.accelerations[j] = point.accelerations[j] * spd * spd
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
def runTrajectory(req):
global Traj_server;
print "---------------------------------"
print req.task
print " "
print req.bin_num
print " "
print req.using_torso
print "---------------------------------"
if req.using_torso == "y":
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num);
else:
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num);
if req.task == "Forward":
file_name = file_root+"/forward";
elif req.task == "Drop":
file_name = file_root+"/drop";
else :
return taskResponse(False);
f = open(file_name,"r");
plan_file = RobotTrajectory();
buf = f.read();
plan_file.deserialize(buf);
plan = copy.deepcopy(plan_file);
arm_right_group = moveit_commander.MoveGroupCommander("arm_right");
arm_left_group = moveit_commander.MoveGroupCommander("arm_left");
arm_left_group.set_start_state_to_current_state();
StartPnt = JointTrajectoryPoint();
StartPnt.positions = arm_left_group.get_current_joint_values();
StartPnt.velocities = [0]*len(StartPnt.positions);
StartPnt. accelerations = [0]*len(StartPnt.positions);
#print StartPnt;
plan.joint_trajectory.points[0] = StartPnt;
#print plan;
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
display_trajectory = moveit_msgs.msg.DisplayTrajectory();
robot = moveit_commander.RobotCommander();
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while", file_name, " is visualized (again)..."
arm_left_group.execute(plan);
print "Trajectory ", file_name, " finished!"
f.close();
return taskResponse(True);
def move():
global prev_state
global req
global pub
global tf_listener
execution_time = 0.5
seconds = rospy.get_time()
seconds -= 0.1
now = rospy.Time.from_sec(seconds)
while True:
try:
#(trans, rot) = tf_listener.lookupTransform("ee_link", "/world", rospy.Time(0))
break
except:
rospy.sleep(0.5)
# Send request for new pose to IK (inverse kinematics) service
print("KODE!!!!!!!!!!!!!!!!!!!!!!")
print("KODE!!!!!!!!!!!!!!!!!!!!!!")
print("KODE!!!!!!!!!!!!!!!!!!!!!!")
print("KODE!!!!!!!!!!!!!!!!!!!!!!")
print("KODE!!!!!!!!!!!!!!!!!!!!!!")
print("KODE!!!!!!!!!!!!!!!!!!!!!!")
print("KODE!!!!!!!!!!!!!!!!!!!!!!")
print("KODE!!!!!!!!!!!!!!!!!!!!!!")
pose_goal = PoseStamped()
pose_goal.header.stamp = rospy.Time.now()
pose_goal.header.frame_id = "world"
pose_goal.pose.orientation.x = 0.889824 #quaternion[0]
pose_goal.pose.orientation.y = -0.446493 #quaternion[1]
pose_goal.pose.orientation.z = -0.0939676 #quaternion[2]
pose_goal.pose.orientation.w = 0.00515905 #quaternion[3]
pose_goal.pose.position.x = -0.2712
pose_goal.pose.position.y = -0.185915
pose_goal.pose.position.z = 0.451176
req.ik_request.pose_stamped = pose_goal
resp = ik_srv.call(req)
print(resp)
# rospy.logerr("Before check error")
# Check if valid robot configuration can be found
if (resp.error_code.val == -31):
print('No IK found!')
return
# Check if we are near a singularity
# Skip movement if that is the case
diff = np.array(resp.solution.joint_state.position) - np.array(prev_state)
max_diff = np.max(diff)
if (max_diff > 1.0):
print('Singularity detected. Skipping!')
# return
# Publish new pose as JointTrajectory
traj = JointTrajectory()
point = JointTrajectoryPoint()
#traj.header.stamp = rospy.Time.now()
traj.header.frame_id = pose_goal.header.frame_id
traj.joint_names = resp.solution.joint_state.name
point.positions = resp.solution.joint_state.position
point.time_from_start.secs = 20
traj.points.append(point)
rospy.sleep(0.5)
pub.publish(traj)
print(traj)
print("should be published")
# Update previous state of robot to current state
prev_state = resp.solution.joint_state.position
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print
"No valid poses received"
return -1
else:
# Create symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Create Modified DH parameters
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
#
# Define Modified DH Transformation matrix
s = {alpha0: 0, a0: 0, d1: 0.75, # row 1
alpha1: -pi / 2, a1: 0.35, d2: 0, q2: q2 - pi / 2, # row 2
alpha2: 0, a2: 1.25, d3: 0, # row 3
alpha3: -pi / 2, a3: -0.054, d4: 1.5, # row 4
alpha4: pi / 2, a4: 0, d5: 0, # row 5
alpha5: -pi / 2, a5: 0, d6: 0, # row 6
alpha6: 0, a6: 0, d7: 0.303, } # row 7
#
# Create individual transformation matrices, better to do this outside of the loop
T0_1 = Matrix([[cos(q1), -sin(q1), 0, a0],
[sin(q1) * cos(alpha0), cos(q1) * cos(alpha0), -sin(alpha0), -sin(alpha0) * d1],
[sin(q1) * sin(alpha0), cos(q1) * sin(alpha0), cos(alpha0), cos(alpha0) * d1],
[0, 0, 0, 1]])
T0_1 = T0_1.subs(s)
T1_2 = Matrix([[cos(q2), -sin(q2), 0, a1],
[sin(q2) * cos(alpha1), cos(q2) * cos(alpha1), -sin(alpha1), -sin(alpha1) * d2],
[sin(q2) * sin(alpha1), cos(q2) * sin(alpha1), cos(alpha1), cos(alpha1) * d2],
[0, 0, 0, 1]])
T1_2 = T1_2.subs(s)
T2_3 = Matrix([[cos(q3), -sin(q3), 0, a2],
[sin(q3) * cos(alpha2), cos(q3) * cos(alpha2), -sin(alpha2), -sin(alpha2) * d3],
[sin(q3) * sin(alpha2), cos(q3) * sin(alpha2), cos(alpha2), cos(alpha2) * d3],
[0, 0, 0, 1]])
T2_3 = T2_3.subs(s)
T3_4 = Matrix([[cos(q4), -sin(q4), 0, a3],
[sin(q4) * cos(alpha3), cos(q4) * cos(alpha3), -sin(alpha3), -sin(alpha3) * d4],
[sin(q4) * sin(alpha3), cos(q4) * sin(alpha3), cos(alpha3), cos(alpha3) * d4],
[0, 0, 0, 1]])
T3_4 = T3_4.subs(s)
T4_5 = Matrix([[cos(q5), -sin(q5), 0, a4],
[sin(q5) * cos(alpha4), cos(q5) * cos(alpha4), -sin(alpha4), -sin(alpha4) * d5],
[sin(q5) * sin(alpha4), cos(q5) * sin(alpha4), cos(alpha4), cos(alpha4) * d5],
[0, 0, 0, 1]])
T4_5 = T4_5.subs(s)
T5_6 = Matrix([[cos(q6), -sin(q6), 0, a5],
[sin(q6) * cos(alpha5), cos(q6) * cos(alpha5), -sin(alpha5), -sin(alpha5) * d6],
[sin(q6) * sin(alpha5), cos(q6) * sin(alpha5), cos(alpha5), cos(alpha5) * d6],
[0, 0, 0, 1]])
T5_6 = T5_6.subs(s)
T6_7 = Matrix([[cos(q7), -sin(q7), 0, a6],
[sin(q7) * cos(alpha6), cos(q7) * cos(alpha6), -sin(alpha6), -sin(alpha6) * d7],
[sin(q7) * sin(alpha6), cos(q7) * sin(alpha6), cos(alpha6), cos(alpha6) * d7],
[0, 0, 0, 1]])
T6_7 = T6_7.subs(s)
T0_7 = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_7
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
# Generate Rrpy matrix for end effector wrt base link
Rrpy = create_rrpy_matrix(roll, pitch, yaw)
# Compensate for rotation discrepancy between DH parameters and Gazebo
# I managed to aling the axes by rotating on z axis by 180 degree and rotating on y axis by -90 degree
Rcorr = rotation_z(pi) * rotation_y(-pi / 2)
# Apply the correction matrix
Rrpy = Rrpy * Rcorr
# Calculating wrist center position wrt base link
#
nx = Rrpy[0, 2]
ny = Rrpy[1, 2]
nz = Rrpy[2, 2]
# d7 = 0.303 which is wc to end effector
wx = px - 0.303 * nx
wy = py - 0.303 * ny
wz = pz - 0.303 * nz
# Calculate joint angles using Geometric IK method
### Calculate theta1 - theta3 - Inverse Position Problem
# theta 1 is the easy one as it is simply tangent angle between wy and wx looking from above
theta1 = atan2(wy, wx)
# theta 2 and theta 3 is relatively tricky as it is hard to visualize. For both ve need to use
# trigonometric calculations for two adjacent triangle
r = sqrt(wx ** 2 + wy ** 2) - 0.35 # radial distance from link2 to wc from above, 0.35 is a1
# Construct the triangle for cosine law. A is the angle in front of corner with angle a,
# B is the angle in front of corner with angle b. For more detail, see writeup.md
A = 1.5011 # sqrt(1.5 **2 + 0.054 **2)
B = sqrt(r ** 2 + (wz - 0.75) ** 2) #
C = 1.25 # a2 = 1.25
# calculate angle a by using cosine law
a = acos((B ** 2 + C ** 2 - A ** 2) / (2 * B * C))
# compensate 90 degree resting angle and the vertical difference between link2 and wc
theta2 = pi / 2 - a - atan2(wz - 0.75, r)
# calculate angle b by using cosine law
b = acos((A ** 2 + C ** 2 - B ** 2) / (2 * A * C))
# compensate 90 degree resting angle and the small drop difference between link3 and wc
# 0.036 radian is necessary to be taken into account for small drop from joint3 to 4. It is simply atan2(0.054,1.5)
theta3 = pi / 2 - (b + 0.036)
print("Theta1, theta2 and theta3 joint angles are calculated")
### Calculate theta4 - theta6 - Inverse Orientation Problem
#Rotation matrix from base link to link 3 derived from transformation matrix
R0_3 = (T0_1 * T1_2 * T2_3)[0:3, 0:3]
# Apply the calculated theta1 - theta 3 values into rotation matrix.
R0_3 = R0_3.evalf(subs={'q1': theta1, 'q2': theta2, 'q3': theta3})
#Calculate the rotation matrix from link3 to link 6 and apply the correction matrix
R3_6 = R0_3.T * Rrpy # in theory inverse and transpose of R0_3 is equal as rotation matrix is orthogonal.
# But sometimes inverse method seems to be causing some numerical problems so I used transpose method.
# each element of R3_6 matrix consist of one or more trigonometric terms.
# Derivation of theta4,5 and 6 from these terms are well explained in the writeup document.
# Simply, first I calculated theta 5 by acos function as, R3_6[1, 2] = cos(q5)
# Then theta4 and 6 are calculated by dividing to terms of the matrix to cancel-out unwanted angle terms.
theta5 = acos(R3_6[1, 2])
if sin(theta5) < 0:
theta4 = atan2(-R3_6[2, 2], R3_6[0, 2])
theta6 = atan2(R3_6[1, 1], -R3_6[1, 0])
else:
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
print("Theta4, theta5 and theta6 joint angles are calculated")
print("Joint angles for eef position", str(x), "are : ", theta1, theta2, theta3, theta4, theta5, theta6)
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
#
#
# Create Modified DH parameters
#
#
# Define Modified DH Transformation matrix
#
#
# Create individual transformation matrices
#
#
# Extract rotation matrices from the transformation matrices
#
#
###
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # link offset
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link length
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')# twist angle
# Define Joint angle symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Create Modified DH parameters
DH_table = {
alpha0: 0, a0: 0, d1: 0.75, q1: q1,
alpha1: rad(-90), a1: 0.35, d2: 0, q2: q2-rad(90),
alpha2: 0, a2: 1.25, d3: 0, q3: q3,
alpha3: rad(-90), a3: -0.054, d4: 1.50, q4: q4,
alpha4: rad(90), a4: 0, d5: 0, q5: q5,
alpha5: rad(-90), a5: 0, d6: 0, q6: q6,
alpha6: 0, a6: 0, d7: 0.303, q7: 0}
# Define Modified DH Transformation matrix
def TF_matrix(alpha, a, d, q):
T = Matrix([[ cos(q), -sin(q), 0, a],
[ sin(q)*cos(alpha), cos(q)*cos(alpha), -sin(alpha), -sin(alpha)*d],
[ sin(q)*sin(alpha), cos(q)*sin(alpha), cos(alpha), cos(alpha)*d],
[ 0, 0, 0, 1]])
return T
# Create individual transformation matrices
T0_1 = TF_matrix(alpha0, a0, d1, q1).subs(DH_table)
T1_2 = TF_matrix(alpha1, a1, d2, q2).subs(DH_table)
T2_3 = TF_matrix(alpha2, a2, d3, q3).subs(DH_table)
T3_4 = TF_matrix(alpha3, a3, d4, q4).subs(DH_table)
T4_5 = TF_matrix(alpha4, a4, d5, q5).subs(DH_table)
T5_6 = TF_matrix(alpha5, a5, d6, q6).subs(DH_table)
T6_G = TF_matrix(alpha6, a6, d7, q7).subs(DH_table)
T0_G = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_G
# G->g
r, p, y = symbols('r p y')
ROT_x = Matrix([[ 1, 0, 0],
[ 0, cos(r), -sin(r)],
[ 0, sin(r), cos(r)]]) # Roll
ROT_y = Matrix([[ cos(p), 0, sin(p)],
[ 0, 1, 0],
[-sin(p), 0, cos(p)]]) # Pitch
ROT_z = Matrix([[ cos(y), -sin(y), 0],
[ sin(y), cos(y), 0],
[ 0, 0, 1]]) # Yaw
ROT_gg = ROT_z * ROT_y * ROT_x
# Compensate for rotation discrepancy between DH parameters and Gazebo (URDF)
# More information can be found in KR210 Forward Kinematics section
Rot_Correct = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_gg = ROT_gg * Rot_Correct
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
#
#
# Calculate joint angles using Geometric IK method
#
#
###
ROT_gg = ROT_gg.subs({'r': roll, 'p': pitch, 'y': yaw})
gg = Matrix([[px], [py], [pz]])
WC = gg - 0.303 * ROT_gg[:,2]
# Calculate joint angles using Geometric IK method
theta1 = atan2(WC[1], WC[0])
# theta3
side_O2_O3 = DH_table[a2]
side_O3_WC = sqrt(DH_table[d4] * DH_table[d4] + DH_table[a3] * DH_table[a3])
size_O2_WC = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - DH_table[a1]), 2) + pow(WC[2] - DH_table[d1], 2))
cos_angle_O2_O3_WC = ((side_O2_O3 * side_O2_O3 + side_O3_WC * side_O3_WC - size_O2_WC * size_O2_WC) / (2 * side_O2_O3 * side_O3_WC))
angle_O2_O3_WC = atan2(sqrt(1 - cos_angle_O2_O3_WC * cos_angle_O2_O3_WC), cos_angle_O2_O3_WC)
angle_p0_O3_O4 = atan2(Abs(DH_table[d4]), Abs(DH_table[a3]))
theta3 = -(angle_O2_O3_WC - angle_p0_O3_O4)
#theta2
cos_angle_O3_O2_WC = ((side_O2_O3 * side_O2_O3 + size_O2_WC * size_O2_WC - side_O3_WC * side_O3_WC) / (2 * side_O2_O3 * size_O2_WC))
angle_O3_O2_WC = atan2(sqrt(1 - cos_angle_O3_O2_WC * cos_angle_O3_O2_WC), cos_angle_O3_O2_WC)
angle_WC_O2_X1 = atan2(WC[2] - Abs(DH_table[d1]), sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - Abs(DH_table[a1]))
theta2 = pi/2 - angle_O3_O2_WC - angle_WC_O2_X1
# theta4, 5, 6
R0_3 = T0_1[0:3,0:3] * T1_2[0:3,0:3] * T2_3[0:3,0:3]
R0_3 = R0_3.evalf(subs={q1:theta1, q2:theta2, q3:theta3})
R3_6 = R0_3.T * ROT_gg
# Euler angles from rotation matrix
# More information can be found in the Euler Angles from a Rotation Matrix section
theta4 = atan2(R3_6[2,2], -R3_6[0,2])
theta6 = atan2(-R3_6[1,1], R3_6[1,0])
theta5 = atan2(R3_6[1,0]/cos(theta6), R3_6[1,2])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols for joint variables
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
# Create Modified DH parameters
s = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: -pi / 2.,
a1: 0.35,
d2: 0,
q2: -pi / 2. + q2,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2.,
a3: -0.054,
d4: 1.5,
q4: q4,
alpha4: pi / 2.,
a4: 0,
d5: 0,
q5: q5,
alpha5: -pi / 2.,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Define Modified DH Transformation matrix
def DH_Tmatrix(alpha, a, d, q):
T = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return T
# Create individual transformation matrices
T0_1 = DH_Tmatrix(alpha0, a0, d1, q1).subs(s)
T1_2 = DH_Tmatrix(alpha1, a1, d2, q2).subs(s)
T2_3 = DH_Tmatrix(alpha2, a2, d3, q3).subs(s)
T3_4 = DH_Tmatrix(alpha3, a3, d4, q4).subs(s)
T4_5 = DH_Tmatrix(alpha4, a4, d5, q5).subs(s)
T5_6 = DH_Tmatrix(alpha5, a5, d6, q6).subs(s)
T6_EE = DH_Tmatrix(alpha6, a6, d7, q7).subs(s)
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
# rotation matrices
r, p, y = symbols('r p y')
R_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0,
sin(r),
cos(r)]])
R_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0,
cos(p)]])
R_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0,
1]])
# R_EE = simplify(R_z * R_y * R_x)
R_EE = R_z * R_y * R_x
R_error = R_z.subs(y, radians(180)) * R_y.subs(p, radians(-90))
R_EE = R_EE * R_error
R_EE = R_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303) * R_EE[:, 2]
theta1 = atan2(WC[1], WC[0])
side_a = 1.501
side_b = sqrt(
pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) +
pow((WC[2] - 0.75), 2))
side_c = 1.25
angle_a = acos(
(side_b * side_b + side_c * side_c - side_a * side_a) /
(2 * side_b * side_c))
angle_b = acos(
(side_a * side_a + side_c * side_c - side_b * side_b) /
(2 * side_a * side_c))
angle_c = acos(
(side_a * side_a + side_b * side_b - side_c * side_c) /
(2 * side_a * side_b))
theta2 = pi / 2 - angle_a - atan2(
WC[2] - 0.75,
sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)
theta3 = pi / 2 - (angle_b + 0.036)
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * R_EE
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(
sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]),
R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def call_TOPP(req):
### Get the request path and prepare for TOPP
t0 = time.time()
print("= Get the request path...")
path_points = req.path
vel_limits = req.vel_limits
acc_limits = req.acc_limits
discrtimestep = req.timestep
# create a path going through viapoints
print("== Create a path going through viapoints...")
dim = len(path_points[0].positions)
ndata = len(path_points)
path_array = np.zeros((dim, ndata))
for i in range(ndata):
path_array[:, i] = np.array(path_points[i].positions)
print("=== Interpolate viapoints...")
#import pdb
#pdb.set_trace()
path = Utilities.InterpolateViapoints(
path_array) # Interpolate using splines
# Constraints
vmax = np.array(vel_limits)
amax = np.array(acc_limits)
### Set up the TOPP instance
print("==== Set up a TOPP instance...")
t1 = time.time()
trajectorystring = str(path)
uselegacy = True # True to use KinematicLimits(vel & acc)
if uselegacy: #Using the legacy KinematicLimits (a bit faster but not fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += "\n" + string.join([str(a) for a in amax])
x = TOPPbindings.TOPPInstance(None, "KinematicLimits",
constraintstring, trajectorystring)
else: #Using the general QuadraticConstraints (fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += TOPPpy.ComputeKinematicConstraints(
path, amax, discrtimestep)
x = TOPPbindings.TOPPInstance(None, "QuadraticConstraints",
constraintstring, trajectorystring)
### Run TOPP
print("===== Run TOPP...")
t2 = time.time()
#import pdb
#pdb.set_trace()
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory()
### Convert resultant array to plan
print("====== Convert resultant array to plan...")
t3 = time.time()
x.WriteResultTrajectory(
) # be sure to write the result before reading it!!!
traj = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
traj_point = JointTrajectoryPoint()
traj_points = [] #PathToTrajResponse() # just an empty list
t = 0.0
while t < traj.duration:
traj_point.positions = traj.Eval(t).tolist()
traj_point.velocities = traj.Evald(t).tolist()
traj_point.accelerations = traj.Evaldd(t).tolist()
traj_point.time_from_start = rospy.Duration(t)
traj_points.append(copy.deepcopy(traj_point))
t += discrtimestep
# the last point
traj_point.positions = traj.Eval(traj.duration).tolist()
traj_point.velocities = traj.Evald(traj.duration).tolist()
traj_point.accelerations = traj.Evaldd(traj.duration).tolist()
traj_point.time_from_start = rospy.Duration(traj.duration)
traj_points.append(copy.deepcopy(traj_point))
### Display statistics
t4 = time.time()
print(">>>>> Statistics of TOPP <<<<<")
print("Dimension of path point: " + str(path_array.shape[0]))
print("Number of data points: " + str(path_array.shape[1]))
print("Using legacy: " + str(uselegacy))
print("Discretization step: " + str(discrtimestep) + " s")
print("Obtain request path and get prepared: " + str(t1 - t0) + " s")
print("Setup TOPP: " + str(t2 - t1) + " s")
print("Run TOPP: " + str(t3 - t2) + " s")
print("Convert resultant array to plan: " + str(t4 - t3) + " s")
print("Total: " + str(t4 - t0) + " s")
print("Trajectory duration before TOPP: " + str(path.duration) + " s")
print("Trajectory duration after TOPP: " + str(traj.duration) + " s")
print(">>>>> End <<<<<")
### Return result
res = PathToTrajResponse()
res.traj = traj_points
return res
def add_point(self, positions, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
return data
if __name__ == '__main__':
# Load the array of path points
#path_points_array = np.array(csv_read('data_real(1).csv'))
path_points_array = np.loadtxt(open("data_real(1).csv", "rb"),
delimiter=",",
skiprows=0)
# Construct a plan of path points
path_points_plan = []
path_point = JointTrajectoryPoint()
for i in range(path_points_array.shape[0]):
path_point.positions = path_points_array[i, 0:5].tolist()
path_points_plan.append(copy.deepcopy(path_point))
# Limits
vel_limits = [math.pi for _ in range(5)]
acc_limits = [math.pi for _ in range(5)] # 5-dim!!!
# Call the server to add time parameterization
import pdb
pdb.set_trace()
new_path_points_plan = add_time_optimal_parameterization_client(
path_points_plan, vel_limits, acc_limits, 0.04)
#TOPPRA_client(path_points_plan, vel_limits, acc_limits, 0.01)
#add_time_optimal_parameterization_client(path_points_plan, vel_limits, acc_limits, 0.04)
# Write to .csv
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# Initialize service response
joint_trajectory_list = []
def DH_transform(q, d, alpha, a):
'''The modified DH convention transform matrix
alpha: twist angle, a: link length,
d: link offset, q: joint angle'''
T = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return T
def rot_x(q):
''' Rotation matrix along x axis'''
R_x = Matrix([[1, 0, 0], [0, cos(q), -sin(q)], [0,
sin(q),
cos(q)]])
return R_x
def rot_y(q):
''' Rotation matrix along y axis'''
R_y = Matrix([[cos(q), 0, sin(q)], [0, 1, 0], [-sin(q), 0,
cos(q)]])
return R_y
def rot_z(q):
''' Rotation matrix along z axis'''
R_z = Matrix([[cos(q), -sin(q), 0], [sin(q), cos(q), 0], [0, 0,
1]])
return R_z
def Euler_angles_from_matrix_URDF_old(R, angles_pre):
''' Calculate q4-6 from R3_6 rotation matrix
Input R is 3x3 rotation matrix, output are Euler angles :q4, q5, q6'''
r12, r13 = R[0, 1], R[0, 2]
r21, r22, r23 = R[1, 0], R[1, 1], R[1, 2]
r32, r33 = R[2, 1], R[2, 2]
# # Euler angles from rotation matrix
# q5 = atan2(sqrt(r13**2 + r33**2), r23)
# q4 = atan2(r33, -r13)
# q6 = atan2(-r22, r21)
if np.abs(r23) is not 1:
q5 = atan2(sqrt(r13**2 + r33**2), r23)
if sin(q5) < 0:
q4 = atan2(-r33, r13)
q6 = atan2(r22, -r21)
else:
q4 = atan2(r33, -r13)
q6 = atan2(-r22, r21)
else:
q6 = angles_pre[5]
if r23 == 1:
q5 = 0
q4 = -q6 + atan2(-r12, -r32)
else:
q5 = 0
q4 = q6 - atan2(r12, -r32)
return np.float64(q4), np.float64(q5), np.float64(q6)
def Euler_angles_from_matrix_URDF(R, angles_pre):
''' Calculate q4-6 from R3_6 rotation matrix
Input R is 3x3 rotation matrix, output are Euler angles :q4, q5, q6'''
r12, r13 = R[0, 1], R[0, 2]
r21, r22, r23 = R[1, 0], R[1, 1], R[1, 2]
r32, r33 = R[2, 1], R[2, 2]
if np.abs(r23) is not 1:
q5 = np.arctan2(np.sqrt(r13**2 + r33**2), r23)
if sin(q5) < 0:
q4 = np.arctan2(-r33, r13)
q6 = np.arctan2(r22, -r21)
else:
q4 = np.arctan2(r33, -r13)
q6 = np.arctan2(-r22, r21)
else:
q6 = angles_pre[5]
if r23 == 1:
q5 = 0
q4 = -q6 + np.arctan2(-r12, -r32)
else:
q5 = 0
q4 = q6 - np.arctan2(r12, -r32)
return q4, q5, q6
def angle_simplify(theta):
''' Transfer the theta into [-2pi, 2pi]'''
return np.float64(np.abs(theta) % (2 * np.pi) * np.sign(theta))
# Define variables
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
r, p, y = symbols("r p y") # end-effector orientation
px_s, py_s, pz_s = symbols("px_s py_s pz_s") # end-effector position
R_corr = rot_z(pi) * rot_y(
-pi / 2) # Compensation matix from URDF to world frame
theta1, theta2, theta3, theta4, theta5, theta6 = 0, 0, 0, 0, 0, 0
angles_pre = (0, 0, 0, 0, 0, 0)
r2d = 180. / np.pi
loop_times = []
# The Modified DH params
s = {
alpha0: 0,
a0: 0,
d1: 0.75,
alpha1: -pi / 2,
a1: 0.35,
d2: 0,
q2: q2 - pi / 2,
alpha2: 0,
a2: 1.25,
d3: 0,
alpha3: -pi / 2,
a3: -0.054,
d4: 1.5,
alpha4: pi / 2,
a4: 0,
d5: 0,
alpha5: -pi / 2,
a5: 0,
d6: 0,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Define Modified DH Transformation matrix
if not os.path.exists("R0_3_inv.p"):
print("-----------------------------------------")
print("No DH matrices, create and save it.")
# base_link to link1
T0_1 = DH_transform(q1, d1, alpha0, a0).subs(s)
# linke1 to link 2
T1_2 = DH_transform(q2, d2, alpha1, a1).subs(s)
# link2 to link3
T2_3 = DH_transform(q3, d3, alpha2, a2).subs(s)
# link3 to link4
T3_4 = DH_transform(q4, d4, alpha3, a3).subs(s)
# link4 to link5
T4_5 = DH_transform(q5, d5, alpha4, a4).subs(s)
# link5 to link6
T5_6 = DH_transform(q6, d6, alpha5, a5).subs(s)
# link6 to end-effector
T6_7 = DH_transform(q7, d7, alpha6, a6).subs(s)
# Create individual transformation matrices
T0_2 = simplify(T0_1 * T1_2)
p2_0_sym = T0_2 * Matrix([0, 0, 0, 1])
# R0_3 inv matrix would be used in R3_6
T0_3 = simplify(T0_2 * T2_3)
R0_3 = T0_3[0:3, 0:3]
R0_3_inv = simplify(R0_3**-1)
# T0_4 = simplify(T0_3 * T3_4)
# T0_5 = simplify(T0_4 * T4_5)
# T0_6 = simplify(T0_5 * T5_6)
# T0_G = simplify(T0_6 * T6_7)
# R_corr = rot_z(pi) * rot_y(-pi/2)
#T_total = simplify(T0_G * R_corr)
# T3_6 = simplify(T3_4*T4_5*T5_6)
R0_g_sym = simplify(rot_z(y) * rot_y(p) * rot_x(r))
# pickle.dump(T0_2, open("T0_2.p", "wb"))
# pickle.dump(T3_6, open("T3_6.p", "wb"))
pickle.dump(p2_0_sym, open("p2_0_sym.p", "wb"))
pickle.dump(R0_3_inv, open("R0_3_inv.p", "wb"))
pickle.dump(R0_g_sym, open("R0_g_sym.p", "wb"))
print("Transformation matrices have been saved!!")
print("-----------------------------------------")
else:
# T0_2 = pickle.load(open("T0_2.p", "rb"))
# T3_6 = pickle.load(open("T3_6.p", "rb"))
p2_0_sym = pickle.load(open("p2_0_sym.p", "rb"))
R0_3_inv = pickle.load(open("R0_3_inv.p", "rb"))
R0_g_sym = pickle.load(open("R0_g_sym.p", "rb"))
print("-----------------------------------------")
print("Transformation matrices have been loaded!")
# Joint angles calculation
pg_0 = Matrix([[px_s], [py_s], [pz_s]])
R0_g = R0_g_sym[0:3, 0:3] * R_corr
pwc_0 = pg_0 - 0.303 * R0_g * Matrix([[0], [0], [1]])
theta1_sym = atan2(pwc_0[1], pwc_0[0]).subs(s)
pwc_2 = pwc_0 - p2_0_sym[0:3, :]
l23 = a2
l35 = sqrt(a3**2 + d4**2)
# l25 = sqrt(np.sum(np.square(pwc_2)))
l25 = sqrt(pwc_2[0]**2 + pwc_2[1]**2 + pwc_2[2]**2)
# Calculate theta2
theta22 = atan2(pwc_2[2], sqrt(pwc_2[0]**2 + pwc_2[1]**2))
c523 = (-l35**2 + l23**2 + l25**2) / (2 * l23 * l25)
theta21 = atan2(sqrt(1 - c523**2), c523)
theta2_sym = (pi / 2 - (theta21 + theta22)).subs(s)
# Calculate theta3
theta31 = atan2(a3, d4)
c235 = (l25**2 - l23**2 - l35**2) / (2 * l23 * l35)
theta32 = atan2(sqrt(1 - c235**2), c235)
theta3_sym = (theta32 + theta31 - pi / 2).subs(s)
# Transfer symbol into numpy function to evaluate an expression more efficient
theta1_f = lambdify((px_s, py_s, pz_s, r, p, y), theta1_sym, "numpy")
theta2_f = lambdify((q1, px_s, py_s, pz_s, r, p, y), theta2_sym,
"numpy")
theta3_f = lambdify((q1, px_s, py_s, pz_s, r, p, y), theta3_sym,
"numpy")
# Calculate R3_6 for theta4-6
R3_6_sym = R0_3_inv * R0_g
R3_6_f = lambdify((q1, q2, q3, px_s, py_s, pz_s, r, p, y), R3_6_sym,
"numpy")
loop_start_time = time.time()
for x in xrange(0, len(req.poses)):
loop_current_time = time.time()
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
# Calculate joint angles using Geometric IK method
theta1 = theta1_f(px, py, pz, roll, pitch, yaw)
theta2 = theta2_f(theta1, px, py, pz, roll, pitch, yaw)
theta3 = theta3_f(theta1, px, py, pz, roll, pitch, yaw)
R3_6 = R3_6_f(theta1, theta2, theta3, px, py, pz, roll, pitch, yaw)
theta4, theta5, theta6 = Euler_angles_from_matrix_URDF(
R3_6, angles_pre)
angles_pre = (theta1, theta2, theta3, theta4, theta5, theta6)
loop_time = time.time() - loop_current_time
loop_times.append(loop_time)
# print("Calculating trajectory:{:2d}, Time cost:{:>2f}".format(x, loop_time))
# Populate response for the IK request
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
print("Inverse kinematics calculation has been done!")
print("Total calculation time: {:>4f}".format(time.time() -
loop_start_time))
print("Average calculation time: {:>4f}".format(np.mean(loop_times)))
print("-----------------------------------------")
print("theta1: {:>4f}".format(theta1 * r2d))
print("theta2: {:>4f}".format(theta2 * r2d))
print("theta3: {:>4f}".format(theta3 * r2d))
print("theta4: {:>4f}".format(theta4 * r2d))
print("theta5: {:>4f}".format(theta5 * r2d))
print("theta6: {:>4f}".format(theta6 * r2d))
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
print("-----------------------------------------")
print("Moving robot arm...")
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(self, req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# Initialize service response
joint_trajectory_list = []
for i in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[i].position.x
py = req.poses[i].position.y
pz = req.poses[i].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[i].orientation.x, req.poses[i].orientation.y,
req.poses[i].orientation.z, req.poses[i].orientation.w
])
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
R0_ee = self.R0_ee.subs({
self.r: roll,
self.p: pitch,
self.y: yaw
})
## Calculate joint angles using Geometric IK method
# 1. find the location of the WC relative to the base frame.
EE = Matrix([[px], [py], [pz]])
WC = (EE - 0.303 * R0_ee[:, 2])
# 2. find theta1, theta2, theta3 from r_WC
theta1 = atan2(WC[1], WC[0])
side_a = 1.501 #sqrt(1.5*1.5 + 0.054*0.054)
side_b = sqrt((sqrt(WC[0]**2 + WC[1]**2) - 0.35)**2 +
(WC[2] - 0.75)**2)
side_c = 1.25 #a2
angle_a = acos((side_b**2 + side_c**2 - side_a**2) /
(2. * side_b * side_c))
angle_b = acos((side_c**2 + side_a**2 - side_b**2) /
(2. * side_c * side_a))
angle_c = acos((side_a**2 + side_b**2 - side_c**2) /
(2. * side_a * side_b))
theta2 = pi / 2. - angle_a - atan2(
WC[2] - 0.75,
sqrt(WC[0]**2 + WC[1]**2) - 0.35)
theta3 = pi / 2. - (angle_b + 0.036
) #0.036 = atan2(abs(a3), d4)
# 3. find R0_3 via application of homogeneous transforms up to the WC.
R0_3 = self.R0_3.subs({
self.q1: theta1,
self.q2: theta2,
self.q3: theta3
})
R3_6 = R0_3.transpose() * R0_ee
# 4. find the final set of Euler angles corresponding to the rotation matrix
# R3_6 = Matrix([
# [-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5), -sin(q5)*cos(q4)],
# [sin(q5)*cos(q6), -sin(q5)*sin(q6), cos(q5)],
# [-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4), sin(q4)*sin(q6)*cos(q5) - cos(q4)*cos(q6), sin(q4)*sin(q5)]])
theta5 = atan2(sqrt(R3_6[0, 2]**2 + R3_6[2, 2]**2), R3_6[1, 2])
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def trapezoidal_speed_trajectory(goal, start,
kv_max, ka_max,
nb_points=100):
"""
Calculate a trajectory from a start state (or current state)
to a goal in joint space using a trapezoidal velocity model
If no kv and ka max are given the default are used
:param goal: A JointState to be used as the goal of the trajectory
:param nb_points: Number of joint-space points in the final trajectory
:param kv_max: max K for velocity,
can be a dictionary joint_name:value or a single value
:param ka_max: max K for acceleration,
can be a dictionary joint_name:value or a single value
:param start: A JointState to be used as the start state,
joint order must be the same as the goal
:return: The corresponding RobotTrajectory
"""
def calculate_coeff(k, dist):
coeff = []
for i in range(len(dist)):
min_value = 1
for j in range(len(dist)):
if i != j:
if k[i] * dist[j] > 0.0001:
min_value = min(min_value, (k[j] * dist[i]) / (k[i] * dist[j]))
coeff.append(min_value)
return coeff
def calculate_max_speed(kv_des, ka, dist):
kv = []
for i in range(len(dist)):
if dist[i] <= 1.5 * kv_des[i] * kv_des[i] / ka[i]:
kv.append(np.sqrt((2.0 / 3) * dist[i] * ka[i]))
else:
kv.append(kv_des[i])
return kv
def calculate_tau(kv, ka, lambda_i, mu_i):
tau = []
for i in range(len(kv)):
if mu_i[i] * ka[i] > 0.0001:
tau.append((3.0 / 2) * (lambda_i[i] * kv[i]) / (mu_i[i] * ka[i]))
else:
tau.append(0.0)
return tau
def calculate_time(tau, lambda_i, kv, dist):
time = []
for i in range(len(tau)):
if kv[i] > 0.0001:
time.append(tau[i] + dist[i] / (lambda_i[i] * kv[i]))
else:
time.append(0.0)
return time
def calculate_joint_values(qi, D, tau, tf, nb_points):
if tf > 0.0001:
q_values = []
time = np.linspace(0, tf, nb_points)
for t in time:
if t <= tau:
q_values.append(qi + D * (1.0 / (2 * (tf - tau))) *
(2 * t**3 / (tau**2) - t**4 / (tau**3)))
elif t <= tf - tau:
q_values.append(qi + D * ((2 * t - tau) / (2 * (tf - tau))))
else:
q_values.append(qi + D * (1 - (tf - t)**3 / (2 * (tf - tau)) *
((2 * tau - tf + t) / (tau**3))))
else:
q_values = np.ones(nb_points) * qi
return q_values
# check that goal and start are not RobotState
if isinstance(goal, RobotState):
goal = goal.joint_state
if isinstance(start, RobotState):
start = start.joint_state
# create the joint trajectory message
jt = JointTrajectory()
joints = []
start_state = start.position
goal_state = [goal.position[goal.name.index(joint)] for joint in start.name]
# calculate the max joint velocity
dist = np.array(goal_state) - np.array(start_state)
abs_dist = np.absolute(dist)
if isinstance(ka_max, dict):
ka = np.ones(len(goal_state)) * map(lambda name: ka_max[name],
goal.name)
else:
ka = np.ones(len(goal_state)) * ka_max
if isinstance(kv_max, dict):
kv = np.ones(len(goal_state)) * map(lambda name: kv_max[name],
goal.name)
else:
kv = np.ones(len(goal_state)) * kv_max
kv = calculate_max_speed(kv, ka, abs_dist)
# calculate the synchronisation coefficients
lambda_i = calculate_coeff(kv, abs_dist)
mu_i = calculate_coeff(ka, abs_dist)
# calculate the total time
tau = calculate_tau(kv, ka, lambda_i, mu_i)
tf = calculate_time(tau, lambda_i, kv, abs_dist)
dt = np.array(tf).max() * (1.0 / nb_points)
if np.array(tf).max() > 0.0001:
# calculate the joint value
for j in range(len(goal_state)):
pose_lin = calculate_joint_values(start_state[j], dist[j],
tau[j], tf[j], nb_points + 1)
joints.append(pose_lin[1:])
for i in range(nb_points):
point = JointTrajectoryPoint()
for j in range(len(goal_state)):
point.positions.append(joints[j][i])
# append the time from start of the position
point.time_from_start = rospy.Duration.from_sec((i + 1) * dt)
# append the position to the message
jt.points.append(point)
else:
point = JointTrajectoryPoint()
point.positions = start_state
point.time_from_start = rospy.Duration.from_sec(0)
# append the position to the message
jt.points.append(point)
# put name of joints to be moved
jt.joint_names = start.name
return jt
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# Initialize service response
joint_trajectory_list = []
# Define DH param symbols
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
# Joint angle symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # joint variables: theta_i
roll, pitch, yaw = symbols('roll pitch yaw')
# Modified DH params
s = {alpha0:0, a0:0, d1:0.75,
alpha1:-pi/2, a1:0.35, d2:0, q2:q2 - pi / 2,
alpha2:0, a2:1.25, d3:0,
alpha3:-pi/2, a3:-0.054, d4:1.50,
alpha4:pi/2, a4:0, d5:0,
alpha5:-pi/2, a5:0, d6:0,
alpha6:0, a6:0, d7:0.303, q7:0}
# Define Modified DH Transformation matrix
T0_1 = Matrix([[ cos(q1), -sin(q1), 0, a0 ],
[ sin(q1)*cos(alpha0), cos(q1)*cos(alpha0), -sin(alpha0), -sin(alpha0)*d1 ],
[ sin(q1)*sin(alpha0), cos(q1)*sin(alpha0), cos(alpha0), cos(alpha0)*d1 ],
[ 0, 0, 0, 1 ]])
T0_1 = T0_1.subs(s)
T1_2 = Matrix([[ cos(q2), -sin(q2), 0, a1 ],
[ sin(q2)*cos(alpha1), cos(q2)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d2 ],
[ sin(q2)*sin(alpha1), cos(q2)*sin(alpha1), cos(alpha1), cos(alpha1)*d2 ],
[ 0, 0, 0, 1 ]])
T1_2 = T1_2.subs(s)
T2_3 = Matrix([[ cos(q3), -sin(q3), 0, a2 ],
[ sin(q3)*cos(alpha2), cos(q3)*cos(alpha2), -sin(alpha2), -sin(alpha2)*d3 ],
[ sin(q3)*sin(alpha2), cos(q3)*sin(alpha2), cos(alpha2), cos(alpha2)*d3 ],
[ 0, 0, 0, 1 ]])
T2_3 = T2_3.subs(s)
T3_4 = Matrix([[ cos(q4), -sin(q4), 0, a3 ],
[ sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), -sin(alpha3)*d4 ],
[ sin(q4)*sin(alpha3), cos(q4)*sin(alpha3), cos(alpha3), cos(alpha3)*d4 ],
[ 0, 0, 0, 1 ]])
T3_4 = T3_4.subs(s)
T4_5 = Matrix([[ cos(q5), -sin(q5), 0, a4 ],
[ sin(q5)*cos(alpha4), cos(q5)*cos(alpha4), -sin(alpha4), -sin(alpha4)*d5 ],
[ sin(q5)*sin(alpha4), cos(q5)*sin(alpha4), cos(alpha4), cos(alpha4)*d5 ],
[ 0, 0, 0, 1 ]])
T4_5 = T4_5.subs(s)
T5_6 = Matrix([[ cos(q6), -sin(q6), 0, a5 ],
[ sin(q6)*cos(alpha5), cos(q6)*cos(alpha5), -sin(alpha5), -sin(alpha5)*d6 ],
[ sin(q6)*sin(alpha5), cos(q6)*sin(alpha5), cos(alpha5), cos(alpha5)*d6 ],
[ 0, 0, 0, 1 ]])
T5_6 = T5_6.subs(s)
T6_G = Matrix([[ cos(q7), -sin(q7), 0, a6 ],
[ sin(q7)*cos(alpha6), cos(q7)*cos(alpha6), -sin(alpha6), -sin(alpha1)*d7 ],
[ sin(q7)*sin(alpha6), cos(q7)*sin(alpha6), cos(alpha6), cos(alpha1)*d7 ],
[ 0, 0, 0, 1 ]])
T6_G = T6_G.subs(s)
# Transform from base link to end effector
T0_2 = T0_1 * T1_2
T0_3 = T0_2 * T2_3
T0_4 = T0_3 * T3_4
T0_5 = T0_4 * T4_5
T0_6 = T0_5 * T5_6
T0_G = T0_6 * T6_G
# Correcting gripper Orientation to be same that of base frame
R_z = rot_z(pi)
R_y = rot_y(-pi/2)
R_corr = R_z * R_y
R0_3 = T0_3[0:3, 0:3]
R0_3_inv = R0_3**(-1)
for i in xrange(0, len(req.poses)):
main_start = datetime.datetime.now()
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[i].position.x
py = req.poses[i].position.y
pz = req.poses[i].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[i].orientation.x, req.poses[i].orientation.y,
req.poses[i].orientation.z, req.poses[i].orientation.w])
# Calculate joint angles using Geometric IK method
WC_start = datetime.datetime.now()
R_roll = rot_x(roll)
R_pitch = rot_y(pitch)
R_yaw = rot_z(yaw)
R0_6 = R_roll * R_pitch * R_yaw * R_corr
# Calculate WC - Wrist Center
# Wrist Center is d7 distance away from End Effector
P = Matrix([px, py, pz])
WC = P - R0_6 * Matrix([0, 0, s[d7]])
WC_end = datetime.datetime.now()
WC_delta = WC_end - WC_start
theta1_3_start = datetime.datetime.now()
# theta1, theta2 and theta3 are are calculated using geometry.
# Calculate theta1
theta1 = atan2(WC[1], WC[0]).evalf()
# Calculate theta2
s1 = sqrt(WC[0]**2 + WC[1]**2) - s[a1]
s2 = WC[2] - s[d1]
s3 = sqrt(s1**2 + s2**2)
s4 = sqrt(s[a3]**2 + s[d4]**2)
beeta1 = atan2(s2, s1)
cos_beeta2 = (s[a2]**2 + s3**2 - s4**2)/(2*s[a2]*s3)
sin_beeta2 = sqrt(1-cos_beeta2**2)
beeta2 = atan2(sin_beeta2, cos_beeta2)
theta2 = (pi/2 - beeta1 - beeta2).evalf()
# Calculate theta3
cos_beeta3 = (s[a2]**2 + s4**2 - s3**2)/(2*s[a2]*s4)
sin_beeta3 = sqrt(1 - cos_beeta3**2)
beeta3 = atan2(sin_beeta3, cos_beeta3)
beeta4 = atan2(-s[a3], s[d4])
theta3 = (pi/2 - beeta4 - beeta3).evalf()
theta1_3_end = datetime.datetime.now()
theta1_3_delta = theta1_3_end - theta1_3_start
# Calculate theta4, 5, 6:
theta4_6_start = datetime.datetime.now()
# Refering to the leassons
R3_6 = R0_3_inv * R0_6
R3_6_num = R3_6.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
theta4 = atan2(R3_6_num[2, 2], -R3_6_num[0, 2]).evalf()
theta5 = atan2(sqrt(1 - R3_6_num[1, 2]**2), R3_6_num[1, 2]).evalf()
theta6 = atan2(-R3_6_num[1, 1], R3_6_num[1, 0]).evalf()
theta4_6_end = datetime.datetime.now()
theta4_6_delta = theta4_6_end - theta4_6_start
main_end = datetime.datetime.now()
main_delta = main_end - main_start
print ('time - main, WC, theta1_3, theta4_6: ', main_delta, WC_delta, theta1_3_delta, theta4_6_delta)
print ('theta1 to 6: ', theta1, theta2, theta3, theta4, theta5, theta6)
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
#here I define the four symbols needed for forward kinematics translations.
#first symbol is the link-offsets which is in the user defined z-axis from #the links i-1 to i
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
#second symbol is the link lengths in user defined x-axis from links i-1 to i
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
#third symbol is twist angle
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
#fourth symbol is theta values for rotational transformations
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Create Modified DH parameters: using the symbols above to help us define transformation matrix with each transformation using two rotations and two translations in the two different axes
#the calculations is shown in pictures in README with the first three pages of illustrations.
DH_Table = {
d1: 0.75,
d2: 0,
d3: 0,
d4: 1.5,
d5: 0,
d6: 0,
d7: 0.303,
a0: 0,
a1: 0.35,
a2: 1.25,
a3: -0.054,
a4: 0,
a5: 0,
a6: 0,
alpha0: 0,
alpha1: -pi / 2,
alpha2: 0,
alpha3: -pi / 2,
alpha4: pi / 2,
alpha5: -pi / 2,
alpha6: 0,
q1: q1,
q2: q2 - pi / 2,
q3: q3,
q4: q4,
q5: q5,
q6: q6,
q7: 0
}
# Define Modified DH Transformation matrix (This matrix was shown multiple times in the lesson to give two translations and two rotations per coordinate frame change)
def TM(alpha, a, d, q):
Mod_DH = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha),
-sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return Mod_DH
# Create individual transformation matrices using the DH Table, copy the matrix multiple times
T0_1 = TM(alpha0, a0, d1, q1).subs(DH_Table)
T1_2 = TM(alpha1, a1, d2, q2).subs(DH_Table)
T2_3 = TM(alpha2, a2, d3, q3).subs(DH_Table)
T3_4 = TM(alpha3, a3, d4, q4).subs(DH_Table)
T4_5 = TM(alpha4, a4, d5, q5).subs(DH_Table)
T5_6 = TM(alpha5, a5, d6, q6).subs(DH_Table)
T6_EE = TM(alpha6, a6, d7, q7).subs(DH_Table)
# finally multiply them all together and you get the T0_EE which is the end effector coordinates in the baselink
#coordinate frame.
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
#
# Extract rotation matrices from the transformation matrices
ROT0_1 = T0_1[0:3, 0:3]
ROT1_2 = T1_2[0:3, 0:3]
ROT2_3 = T2_3[0:3, 0:3]
ROT3_4 = T3_4[0:3, 0:3]
ROT4_5 = T04_5[0:3, 0:3]
ROT5_6 = T5_6[0:3, 0:3]
ROT6_EE = T6_EE[0:3, 0:3]
#
#
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
#I'm assuming this is the correction between the URDF and the DH parameter axis for the gripper
#We want to rotate about the z-xis 180 degrees and then around the y-axis by -90 degrees.
#this graphic is shown in README
#due to this rotation being extrinsic, we would reverse the intrinsic operation normally done on an airplane which would be roll, pitch and yaw. So to reverse x-y-z, we will find the rotation of the end effector and its correction using the opposite operations of yaw, pitch and then roll (z,y,x) and then times the correction between urdf and our analysis.
r, p, y = symbols('r p y')
ROT_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)],
[0, sin(r), cos(r)]]) #roll
ROT_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0],
[0, 0, 1]]) #yaw
ROT_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0],
[-sin(p), 0, cos(p)]]) #pitch
R_corr_gripper = ROT_z.subs({y: pi}) * ROT_y.subs({p: -pi / 2})
ROT_EE = ROT_z * ROT_y * ROT_x * R_corr_gripper
#now sub in the given values using a dictionary
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
# Calculate joint angles using Geometric IK method
#First find the wrist center using the rotation matrix of the end effector and subtracting d7 from the end effector position to give us WC position and orientation (which, the orientation of the WC is the same at the end effector in the z-axis direction)
EE = Matrix([[px], [py], [pz]])
WC = EE - .303 * ROT_EE[:, 2]
#d = .303
#ROT_EE[0:2]
#now that we know WC, we do trigonmetric analysis to find theta1, theta2, theta3
# to find theta1 we know that WCx and WCy are adjacent and opposite. since joint1 rotates about the z-axis, its movement is in the #xy-plane. So, we project the movement on xy-place and using SOHCAHTOA => tan^-1(O/A). using atan2 since my mentor said it was better. I googled it and found out atan2 actually helps determine quadrants. Very helpful since I was getting wild paths and my errors were not low enough
theta1 = atan2(WC[1], WC[0])
#to find theta2 we need to set theta1 to zero and draw out the robot in the xz-plane. See figure in write up.
#we use law of cosines to find triangle angle and side values
side_A = sqrt((.054)**2.0 + (1.5)**2.0)
side_B = sqrt((sqrt(WC[0]**2 + WC[1]**2) - .350)**2 +
(WC[2] - .75)**2)
side_C = 1.25
#supporting angles to find theta2 and theta3 using law of cosines
angle_a = acos(
(side_B**2 + side_C**2 - side_A**2) / (2 * side_B * side_C))
angle_b = acos(
(side_A**2 + side_C**2 - side_B**2) / (2 * side_A * side_C))
angle_c = acos(
(side_A**2 + side_B**2 - side_C**2) / (2 * side_A * side_B))
#supporting angle for finding theta2 (see write up picture)
angle_aprime = atan2((WC[2] - .75),
(sqrt(WC[0]**2 + WC[1]**2) - .35))
theta2 = pi / 2 - angle_a - angle_aprime
#to find theta3 we set theta2 and theta1 equal to zero and analyze over xz-plane
theta3 = pi / 2 - angle_b - atan2(.054, 1.5)
#now find theta4,theta5,theta6
#we know that the Rotation matrix for 3/6 = the inverse rotation matrix of joints 0/3 times the rotation matrix for joints 0/6
#See write up for proof. also needed to understand the LU inversion as talked about in the course: https://docs.sympy.org/0.7.0/modules/matrices.html
#T0_3 = T0_1 * T1_2 * T2_3
#ROT0_3 = T0_3[:3,:3]
ROT0_3 = ROT0_1 * ROT1_2 * ROT2_3
ROT0_3 = ROT0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
#T3_EE = T3_4*T4_5*T5_6*T6_EE
ROT3_EE = ROT0_3.transpose() * ROT_EE
#OK now we have our rotation matrix from joints 4-6
#For these angles we can think about that each roll, pitch and yaw is done on a seperate axis which is described
#by a column of the rotation matrix. From the lecture "Euler angles from a rotation matrix" We find:
#theta4 is represented by the roll and gamma = arctan(r32, r33)
#theta 5 is represented by pitch or beta
#theta6 is represented by yaw of the end effector or alpha
theta4 = atan2(ROT3_EE[2, 2], -ROT3_EE[0, 2])
theta5 = atan2(sqrt(ROT3_EE[0, 2]**2 + ROT3_EE[2, 2]**2),
ROT3_EE[1, 2])
theta6 = atan2(-ROT3_EE[1, 1], ROT3_EE[1, 0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def main(whole_body, gripper, wrist_wrench):
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
## Grab the handle of door
target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point",
PoseStamped)
recog_pos.pose.position.x = target_pose_Msg.pose.position.x
recog_pos.pose.position.y = target_pose_Msg.pose.position.y
recog_pos.pose.position.z = target_pose_Msg.pose.position.z
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
y=recog_pos.pose.position.y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.008)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
# print(whole_body.impedance_config)
# desired_rot=-1.95
# whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
wrist_roll = latest_positions["wrist_roll_joint"] - 0.55
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = wrist_roll
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(5.0)
# whole_body.end_effector_frame = u'odom'
# whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
# publish_arm(latest_positions["arm_lift_joint"],latest_positions["arm_flex_joint"],latest_positions["arm_roll_joint"], latest_positions["wrist_flex_joint"],wrist_roll)
# whole_body.end_effector_frame = u'base_link'
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0, 0, 1), 0.35)
whole_body.impedance_config = None
##
# whole_body.move_to_joint_positions({"wrist_roll_joint": -0.3})
# rospy.sleep(2.0)
# whole_body.end_effector_frame = u'odom'
# whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
# whole_body.move_end_effector_by_arc(geometry.pose(x=0.2, y=0.25, z=0.38, ej=math.radians(90.0)), math.radians(60.0))
# whole_body.move_end_effector_by_arc(geometry.pose(y=0.45, z=0.08, ej=math.radians(90.0)), math.radians(60.0), ref_frame_id='hand_palm_link')
# listener = tf.TransformListener()
# listener.waitForTransform("/odom", "/hand_palm_link", rospy.Time().now(), rospy.Duration(3.0))
# now = rospy.Time.now()
# listener.waitForTransform("/odom", "/hand_palm_link", now, rospy.Duration(5.0))
# (trans, rot) = listener.lookupTransform("/map", "/hand_palm_link", now)
# cur_tuples = geometry.transform_to_tuples(target_trans)
# print trans
# print rot
# Rotate the handle (Angle: math.pi/6)
# wrist_wrench.reset()
# whole_body.end_effector_frame = 'hand_palm_link'
# whole_body.impedance_config= 'dumper_soft'
#############################
# odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF)
# tsr_to_odom = geometry.pose(x=-(recog_pos.pose.position.x+HANDLE_TO_HAND_POS),
# y=-(recog_pos.pose.position.y+HANDLE_TO_HANDLE_HINGE_POS),
# z=-recog_pos.pose.position.z)
# tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand)
# const_tsr = TaskSpaceRegion()
# const_tsr.end_frame_id = _HAND_TF
# const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x+HANDLE_TO_HAND_POS,
# y=recog_pos.pose.position.y+HANDLE_TO_HANDLE_HINGE_POS,
# z=recog_pos.pose.position.z))
# const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# const_tsr.min_bounds = [0, 0.0, 0.0,-(math.pi/7) , 0, 0]
# const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, 0]
# goal_tsr = TaskSpaceRegion()
# goal_tsr.end_frame_id = _HAND_TF
# goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x+HANDLE_TO_HAND_POS,
# y=recog_pos.pose.position.y+HANDLE_TO_HANDLE_HINGE_POS,
# z=recog_pos.pose.position.z))
# goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# goal_tsr.min_bounds = [0, 0.0, 0.0,-math.pi/7, 0, 0]
# goal_tsr.max_bounds = [0, 0.0, 0.0,-math.pi/7, 0, 0]
# response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
# if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
# rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
# exit(-1)
# response.base_solution.header.frame_id = _ORIGIN_TF
# constrain_traj = whole_body._constrain_trajectories(response.solution,
# response.base_solution)
# # wrist_wrench.reset()
# # switch.activate("grasping")
# whole_body._execute_trajectory(constrain_traj)
# # whole_body.impedance_config= 'dumper_soft'
# # switch.inactivate()
# rospy.sleep(10.0)
# listener = tf.TransformListener()
# now = rospy.Time.now()
# listener.waitForTransform("/odom", "/hand_palm_link", now, rospy.Duration(3.0))
# rospy.sleep(3.0)
# # Open the door (Angle: math.pi/4)
# #rist_wrench.reset()
# # switch.activate("placing")
# odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF) #T0h
# tsr_to_odom = geometry.pose(x=-(recog_pos.pose.position.x),
# y=-(recog_pos.pose.position.y+HANDLE_TO_DOOR_HINGE_POS+HANDLE_GOAL_OFFSET),
# z=-recog_pos.pose.position.z) #Twe
# tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand) #T0s'
# const_tsr = TaskSpaceRegion()
# const_tsr.end_frame_id = _HAND_TF
# const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x,
# y=recog_pos.pose.position.y+HANDLE_TO_DOOR_HINGE_POS+HANDLE_GOAL_OFFSET,
# z=recog_pos.pose.position.z))
# const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# const_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, 0]
# const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
# goal_tsr = TaskSpaceRegion()
# goal_tsr.end_frame_id = _HAND_TF
# goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x,
# y=recog_pos.pose.position.y+HANDLE_TO_DOOR_HINGE_POS+HANDLE_GOAL_OFFSET,
# z=recog_pos.pose.position.z))
# goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# goal_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
# goal_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
# response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
# if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
# rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
# exit(-1)
# response.base_solution.header.frame_id = _ORIGIN_TF
# constrain_traj = whole_body._constrain_trajectories(response.solution,
# response.base_solution)
# whole_body._execute_trajectory(constrain_traj)
# # whole_body.impedance_config= None
# # switch.inactivate()
gripper.command(1.0)
whole_body.move_to_neutral()
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
self._get_trajectory_parameters(joint_names, goal)
num_joints = len(joint_names)
trajectory_points = goal.trajectory.points
pnt_times = [0.0]*len(trajectory_points)
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (action_name,))
self._server.set_aborted()
return
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
trajectory_points[-1].velocities = [0.0] * len(joint_names)
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
if (num_points == 1) or (pnt_times[0] > 0.0):
# Add current position as trajectory point
first_trajectory_point = JointTrajectoryPoint()
first_trajectory_point.positions = self._get_current_position(joint_names)
# To preserve desired velocities and accelerations, copy them to the first
# trajectory point if the trajectory is only 1 point.
first_trajectory_point.time_from_start = rospy.Duration(0)
trajectory_points.insert(0, first_trajectory_point)
num_points = len(trajectory_points)
for i in range(num_points):
trajectory_points[i].velocities = [0.0] * len(joint_names)
trajectory_points[i].accelerations = [0.0] * len(joint_names)
for i in range(1,num_points):
for j in range(num_joints):
if ((pnt_times[i] - pnt_times[i-1]) > 0.0):
trajectory_points[i].velocities[j] = (trajectory_points[i].positions[j]-trajectory_points[i-1].positions[j])/(pnt_times[i] - pnt_times[i-1])
"""
Wait for the specified execution time, if not provided use now
"""
start_time = goal.trajectory.header.stamp.to_sec()
now = rospy.get_time()
if start_time == 0.0:
start_time = rospy.get_time()
while start_time > now:
now = rospy.get_time()
"""
Loop until end of trajectory time. Provide a single time step
of the control rate past the end to ensure we get to the end.
Keep track of current indices for spline segment generation
"""
control_rate = rospy.Rate(self._trajectory_control_rate)
now_from_start = rospy.get_time() - start_time
end_time = trajectory_points[-1].time_from_start.to_sec()
last_idx = 0
slewed_pos = self._get_current_position(joint_names)
self._command_joints(joint_names, deepcopy(trajectory_points[0]))
while (now_from_start < end_time and not rospy.is_shutdown() and
self.robot_is_enabled()):
now = rospy.get_time()
now_from_start = now - start_time
for i in range(num_points):
if (pnt_times[i] >= now_from_start):
idx = i
break
if (idx >= num_points):
idx = num_points-1
for j in range(num_joints):
slewed_pos[j] += trajectory_points[idx].velocities[j] * (1.0/self._trajectory_control_rate)
point = deepcopy(trajectory_points[idx])
point.positions = slewed_pos
"""
Command Joint Position, Velocity, Acceleration
"""
command_executed = self._command_joints(joint_names, point)
self._update_feedback(deepcopy(point), joint_names, now_from_start)
"""
Break the loop if the command cannot be executed
"""
if not command_executed:
return
control_rate.sleep()
"""
Keep trying to meet goal until goal_time constraint expired
"""
last = trajectory_points[-1]
last_time = trajectory_points[-1].time_from_start.to_sec()
end_angles = dict(zip(joint_names, last.positions))
while (now_from_start < (last_time + self._goal_time)
and not rospy.is_shutdown() and self.robot_is_enabled()):
if not self._command_joints(joint_names, last):
return
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names,
now_from_start)
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names,
now_from_start)
"""
Verify goal constraint
"""
result = self._check_goal_state(joint_names, last)
if result is True:
rospy.loginfo("%s: Joint Trajectory Action Succeeded" %self._action_name)
self._result.error_code = self._result.SUCCESSFUL
self._server.set_succeeded(self._result)
elif result is False:
rospy.logerr("%s: Exceeded Max Goal Velocity Threshold" %self._action_name)
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
else:
rospy.logerr("%s: Exceeded Goal Threshold Error %s"%(self._action_name, result))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
self._command_stop()
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
# Joint angles
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# DH
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # offset for links
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # length of links
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') # twist angle
# EE Poses
r, p, y = symbols('r p y')
# Create Modified DH parameters
s = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: rad(-90),
a1: 0.35,
d2: 0,
q2: q2 - rad(90),
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: rad(-90),
a3: -0.054,
d4: 1.50,
q4: q4,
alpha4: rad(90),
a4: 0,
d5: 0,
q5: q5,
alpha5: rad(-90),
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Create individual transformation matrices
T_0_1 = get_transformation_matrix(alpha0, a0, d1, q1).subs(s)
T_1_2 = get_transformation_matrix(alpha1, a1, d2, q2).subs(s)
T_2_3 = get_transformation_matrix(alpha2, a2, d3, q3).subs(s)
T_3_4 = get_transformation_matrix(alpha3, a3, d4, q4).subs(s)
T_4_5 = get_transformation_matrix(alpha4, a4, d5, q5).subs(s)
T_5_6 = get_transformation_matrix(alpha5, a5, d6, q6).subs(s)
T_6_EE = get_transformation_matrix(alpha6, a6, d7, q7).subs(s)
# Extract rotation matrices from the transformation matrices
T_0_EE = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5 * T_5_6 * T_6_EE
# Compensate for rotation discrepancy between DH parameters and Gazebo
R_x = get_rotation_matrix('roll', r)
R_y = get_rotation_matrix('pitch', p)
R_z = get_rotation_matrix('yaw', y)
# Initialize rotation matrix of End Effector
R_EE = R_z * R_y * R_x
###
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
### Your IK code here
# Calculate ee values and correct rotation matrix of EE
R_EE, P_WC, theta1, theta2, theta3 = calculate_ee(
R_EE, px, py, pz, roll, pitch, yaw)
# Inverse kinematic rotation matrix from wraist to EE
R_0_3 = T_0_1[0:3, 0:3] * T_1_2[0:3, 0:3] * T_2_3[0:3, 0:3]
R_0_3 = R_0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R_3_6 = Transpose(R_0_3) * R_EE
# Angles from rotation matrix
theta4 = atan2(R_3_6[2, 2], -R_3_6[0, 2])
theta5 = atan2(
sqrt(R_3_6[0, 2] * R_3_6[0, 2] + R_3_6[2, 2] * R_3_6[2, 2]),
R_3_6[1, 2])
theta6 = atan2(-R_3_6[1, 1], R_3_6[1, 0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def trajopt_plan(self,
target_pose,
json_config_str=None,
json_config_name=None,
target_joints=None):
with self.lock:
if (json_config_str is None and json_config_name is not None):
json_config_str = self.load_json_config(json_config_name)
robot = self.controller_commander.rox_robot
#vel_upper_lim = np.array(robot.joint_vel_limit) * speed_scalar
#vel_lower_lim = -vel_upper_lim
#joint_lower_limit = np.array(robot.joint_lower_limit)
#joint_upper_limit = np.array(robot.joint_upper_limit)
joint_names = robot.joint_names
joint_positions = self.controller_commander.get_current_joint_values(
)
if target_pose is not None:
p = PoseArray()
p.header.frame_id = "world"
p.header.stamp = rospy.Time.now()
p.poses.append(
rox_msg.transform2pose_msg(
self.controller_commander.compute_fk(joint_positions)))
p.poses.append(rox_msg.transform2pose_msg(target_pose))
self.waypoint_plotter.publish(p)
self.tesseract_env.setState(joint_names, joint_positions)
init_pos = self.tesseract_env.getCurrentJointValues()
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
np.reshape(init_pos, (1, 6)))
planner = tesseract.TrajOptPlanner()
manip = "move_group"
end_effector = "vacuum_gripper_tool"
pci = tesseract.ProblemConstructionInfo(self.tesseract_env)
pci.fromJson(json_config_str)
pci.kin = self.tesseract_env.getManipulator(manip)
pci.init_info.type = tesseract.InitInfo.STATIONARY
#pci.init_info.dt=0.5
if target_pose is not None:
#Final target_pose constraint
pose_constraint = tesseract.CartPoseTermInfo()
pose_constraint.term_type = tesseract.TT_CNT
pose_constraint.link = end_effector
pose_constraint.timestep = pci.basic_info.n_steps - 1
q = rox.R2q(target_pose.R)
pose_constraint.wxyz = np.array(q)
pose_constraint.xyz = np.array(target_pose.p)
pose_constraint.pos_coefs = np.array(
[1000000, 1000000, 1000000], dtype=np.float64)
pose_constraint.rot_coefs = np.array([10000, 10000, 10000],
dtype=np.float64)
pose_constraint.name = "final_pose"
pci.cnt_infos.push_back(pose_constraint)
elif target_joints is not None:
joint_constraint = tesseract.JointPosTermInfo()
joint_constraint.term_type = tesseract.TT_CNT
joint_constraint.link = end_effector
joint_constraint.first_step = pci.basic_info.n_steps - 2
joint_constraint.last_step = pci.basic_info.n_steps - 1
#joint_constraint.coeffs = tesseract.DblVec([10000]*6)
joint_constraint.targets = tesseract.DblVec(
list(target_joints))
print target_joints
pci.cnt_infos.push_back(joint_constraint)
else:
assert False
prob = tesseract.ConstructProblem(pci)
config = tesseract.TrajOptPlannerConfig(prob)
config.params.max_iter = 100000
planning_response = planner.solve(config)
if (planning_response.status_code != 0):
raise Exception(
"TrajOpt trajectory planning failed with code: %d" %
planning_response.status_code)
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
planning_response.trajectory[:, 0:6])
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = joint_names
trajectory_time = np.cumsum(1.0 /
planning_response.trajectory[:, 6])
trajectory_time = trajectory_time - trajectory_time[0]
for i in xrange(planning_response.trajectory.shape[0]):
jt_p = JointTrajectoryPoint()
jt_p.time_from_start = rospy.Duration(trajectory_time[i])
jt_p.positions = planning_response.trajectory[i, 0:6]
jt.points.append(jt_p)
return jt
def make_trajectory_point(duration, position):
point = JointTrajectoryPoint()
point.positions = [position] * 7
point.velocities = [0] * 7
point.time_from_start = rospy.Duration.from_sec(duration)
return point
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') #for theta values
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') #Twisting angles
# Create Modified DH parameters
DH_T = {
alpha0: 0,
a0: 0,
d1: 0.75,
alpha1: -pi / 2,
a1: 0.35,
d2: 0,
q2: q2 - pi / 2,
alpha2: 0,
a2: 1.25,
d3: 0,
alpha3: -pi / 2,
a3: -0.054,
d4: 1.5,
alpha4: pi / 2,
a4: 0,
d5: 0,
alpha5: -pi / 2,
a5: 0,
d6: 0,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Define Modified DH Transformation matrix
# Create individual transformation matrices
TF0_1 = matrix(alpha0, a0, d1, q1).subs(DH_T)
TF1_2 = matrix(alpha1, a1, d2, q2).subs(DH_T)
TF2_3 = matrix(alpha2, a2, d3, q3).subs(DH_T)
TF3_4 = matrix(alpha3, a3, d4, q4).subs(DH_T)
TF4_5 = matrix(alpha4, a4, d5, q5).subs(DH_T)
TF5_6 = matrix(alpha5, a5, d6, q6).subs(DH_T)
TF6_G = matrix(alpha6, a6, d7, q7).subs(DH_T)
TF0_G = simplify(TF0_1 * TF1_2 * TF2_3 * TF3_4 * TF4_5 * TF5_6 * TF6_G)
R_corr = simplify(rot_z(pi) * rot_y(-pi / 2))
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
# rotation matrix for EE
Rot_EE = rot_z(yaw)[0:3, 0:3] * rot_y(pitch)[0:3, 0:3] * rot_x(
roll)[0:3, 0:3] * R_corr[0:3, 0:3]
EE_Pos = Matrix([[px], [py], [pz]])
WC = EE_Pos - 0.303 * Rot_EE[:, 2] # EE relative to WC
# Calculate theta 1 directly
theta_1 = atan2(WC[1], WC[0])
# calculate theta 2
new_wx = sqrt(WC[0]**2 + WC[1]**2) - 0.35
new_wz = WC[2] - 0.75 # WC_Z - d1
B = sqrt(new_wx**2 + new_wz**2)
# A and C fixed from urdf
C = 1.25
A = 1.5
angle_a = math.acos(
(pow(B, 2) + pow(C, 2) - pow(A, 2)) / (2 * B * C))
theta_2 = pi / 2 - angle_a - atan2(new_wz, new_wx)
# to get theta 3 we have to calculate angle_b first as follows:-
angle_b = math.acos(
(pow(C, 2) + pow(A, 2) - pow(B, 2)) / (2 * C * A))
theta_3 = pi / 2 - angle_b - 0.03598 # 0.03598 is fixed angle = atan2(0.054,1.5)
# cal theta 3 -> 6
TF0_2 = simplify(TF0_1 * TF1_2)
TF0_3 = simplify(TF0_2 * TF2_3)
R0_3 = TF0_3.evalf(subs={
q1: theta_1,
q2: theta_2,
q3: theta_3
})[0:3, 0:3]
R3_6 = R0_3.inv("LU") * Rot_EE
theta_4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta_5 = atan2(
sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]),
R3_6[1, 2])
theta_6 = atan2(-R3_6[1, 1], R3_6[1, 0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta_1, theta_2, theta_3, theta_4, theta_5, theta_6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
rospy.init_node("simple_move_goal_pub")
pub = rospy.Publisher("/arm_controller/follow_joint_trajectory/goal", FollowJointTrajectoryActionGoal, queue_size=20)
rospy.sleep(0.5)
topic_name = rospy.resolve_name("/arm_controller/follow_joint_trajectory/goal")
rospy.loginfo("Sending goal to %s", topic_name)
traj = JointTrajectory()
traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
now = rospy.get_rostime()
rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
traj.header.stamp = now
home = JointTrajectoryPoint()
home.positions = home_position
home.velocities = home_velocity
home.time_from_start = rospy.Duration(5.0/2)
traj.points.append(home)
shelf_A = JointTrajectoryPoint()
shelf_A.positions = [math.radians(100.63), math.radians(-106.49), math.radians(89.88), math.radians(-73.64), math.radians(274.08) ,math.radians(11.66)]
shelf_A.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
shelf_A.time_from_start = rospy.Duration(10.0/2)
traj.points.append(shelf_A)
shelf_ATLL = JointTrajectoryPoint()
shelf_ATLL.positions = [math.radians(118.62), math.radians(-94.75), math.radians(73.14), math.radians(-63.7), math.radians(275.07) ,math.radians(48.84)]
shelf_ATLL.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
shelf_ATLL.time_from_start = rospy.Duration(15.0/2)
traj.points.append(shelf_ATLL)
def add_point(self, positions, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = [0.0] * len(self._goal.trajectory.joint_names)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def moveArm(self, flag, data):
rospy.loginfo("received the following point " + str(data))
# Get the current pose so we can add it as a waypoint
j = 0
while (j < 3):
start_pose = self.arm.get_current_pose(self.end_effector_link).pose
print(start_pose)
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
if (flag):
if (j == 0):
eef_step = 0.02
self.goal.command.position = 0.0 # From 0.0 to 0.8
self.goal.command.max_effort = -1 # Do not limit the effort
self.client.send_goal(self.goal)
self.client.wait_for_result()
time.sleep(0.5)
# Set the next waypoint to location of the pick object
wpose.position.x = data.x - 0.025
wpose.position.y = data.y - 0.025
wpose.position.z = travel_height
wpose.orientation.x = -1
wpose.orientation.y = 0
wpose.orientation.z = 1
wpose.orientation.w = 0
waypoints.append(deepcopy(wpose))
elif (j == 1):
eef_step = 0.01
wpose.position.x = data.x
wpose.position.y = data.y
wpose.position.z = data.depth + end_effector_length
wpose.orientation.x = -1
wpose.orientation.y = 0
wpose.orientation.z = 1
wpose.orientation.w = 0
waypoints.append(deepcopy(wpose))
else:
eef_step = 0.02
self.goal.command.position = 0.37 # From 0.0 to 0.8
self.goal.command.max_effort = -1 # Do not limit the effort
self.client.send_goal(self.goal)
self.client.wait_for_result()
time.sleep(0.5)
# raise
wpose.position.x = data.x
wpose.position.y = data.y
wpose.position.z = travel_height
waypoints.append(deepcopy(wpose))
#move to the bin
wpose.position.x = bucket_x
wpose.position.y = bucket_y
wpose.position.z = travel_height
wpose.orientation.x = -1
wpose.orientation.y = 0
wpose.orientation.z = 0.5
wpose.orientation.w = 0
waypoints.append(deepcopy(wpose))
else:
eef_step = 0.01
wpose.position.x = start_pose.position.x - 0.3
wpose.position.y = start_pose.position.y + 0.1
wpose.position.z = travel_height
waypoints.append(deepcopy(wpose))
wpose.position.x = start_pose.position.x - 0.6
wpose.position.y = start_pose.position.y + 0.2
wpose.position.z = travel_height
wpose.orientation.x = -1
wpose.orientation.y = 0
wpose.orientation.z = 0.5
wpose.orientation.w = 0
waypoints.append(deepcopy(wpose))
wpose.position.x = bucket_x
wpose.position.y = bucket_y
wpose.position.z = travel_height
waypoints.append(deepcopy(wpose))
j = 3
fraction = 0.0
maxtries = 100
attempts = 0
j = j + 1
# Set the internal state to the current state
self.arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, eef_step, 0.0, True)
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction > 0.8:
rospy.loginfo("Path computed successfully. Moving the arm.")
num_pts = len(plan.joint_trajectory.points)
rospy.loginfo("\n# waypoints: " + str(num_pts))
waypoints = []
for i in range(num_pts):
waypoints.append(plan.joint_trajectory.points[i].positions)
#rospy.init_node('send_joints')
pub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
# Create the topic message
traj = JointTrajectory()
traj.header = Header()
# Joint names for UR5
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rate = rospy.Rate(20)
cnt = 0
pts = JointTrajectoryPoint()
while not rospy.is_shutdown() and cnt < num_pts - 1:
# print(pub.get_Status())
cnt += 1
traj.header.stamp = rospy.Time.now()
# pts.positions = [0.0, -2.33, 1.57, 0.0, 0.0, 0.0]
pts.positions = waypoints[cnt]
pts.time_from_start = rospy.Duration(0.001 * cnt)
# Set the points to the trajectory
traj.points = []
traj.points.append(pts)
# Publish the message
pub.publish(traj)
rate.sleep()
# self.arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
self.goal.command.position = 0.0 # From 0.0 to 0.8
self.goal.command.max_effort = -1 # Do not limit the effort
self.client.send_goal(self.goal)
self.client.wait_for_result()
def test_trajectories(self):
"""Test robot movement"""
#### Power cycle the robot in order to make sure it is running correctly####
self.assertTrue(self.set_robot_to_mode(RobotMode.POWER_OFF))
rospy.sleep(0.5)
self.assertTrue(self.set_robot_to_mode(RobotMode.RUNNING))
rospy.sleep(0.5)
self.send_program_srv.call()
rospy.sleep(0.5) # TODO properly wait until the controller is running
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-0.5 for i in range(6)])
position_list.append([-1.0 for i in range(6)])
duration_list = [6.0, 9.0, 12.0]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending simple goal")
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.SUCCESSFUL)
rospy.loginfo("Received result SUCCESSFUL")
"""Test trajectory server. This is more of a validation test that the testing suite does the
right thing."""
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-0.5 for i in range(6)])
# Create illegal goal by making the second point come earlier than the first
duration_list = [6.0, 3.0]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending illegal goal")
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
# As timings are illegal, we expect the result to be INVALID_GOAL
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.INVALID_GOAL)
rospy.loginfo("Received result INVALID_GOAL")
"""Test robot movement"""
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-1.0 for i in range(6)])
duration_list = [6.0, 6.5]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending scaled goal without time restrictions")
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.SUCCESSFUL)
rospy.loginfo("Received result SUCCESSFUL")
# Now do the same again, but with a goal time constraint
rospy.loginfo("Sending scaled goal with time restrictions")
goal.goal_time_tolerance = rospy.Duration(0.01)
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED)
rospy.loginfo("Received result GOAL_TOLERANCE_VIOLATED")
hand_gesture3.points = [JointTrajectoryPoint(positions=thumbs_up)]
hand_gesture4 = JointTrajectory()
hand_gesture4.points = [JointTrajectoryPoint(positions=fist)]
hand_gesture5 = JointTrajectory()
hand_gesture5.points = [JointTrajectoryPoint(positions=open_hand)]
arm_names = ['shoulder_medial_joint', 'elbow_joint']
raised = [1, 1]
lowered = [1.6, 1.6]
arm_gesture1 = JointTrajectory()
arm_gesture1.joint_names = arm_names
arm_gesture_points1 = JointTrajectoryPoint()
arm_gesture_points1.positions = [raised]
arm_gesture1.points = [arm_gesture_points1]
arm_gesture2 = JointTrajectory()
arm_gesture2.joint_names = arm_names
arm_gesture_points2 = JointTrajectoryPoint()
arm_gesture_points2.positions = [lowered]
arm_gesture2.points = [arm_gesture_points2]
# ================== #
# Callback Functions #
# ----------------------------------------------------------------------- #
# Evaluates an action and sets the gesture points to reflect that action. #
# ======================================================================= #
def setHandGesture(msg):
global hand_gesture
def gen_traj(joint_limits=np.array([[-math.pi, math.pi]] * 6),
vel_limit=0.1,
acc_mod=0.1,
time_step=0.1,
action_step=10,
duration=10):
traj = JointTrajectory()
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
traj.header.frame_id = '/world'
t = 0.
iters = 0
init_pose = np.array(
last_js.position
) ##### TODO: remove the dependency on global pose ####
init_pose[0:3] = init_pose[2::-1]
pos = np.array([0.] * 6)
vel = np.array([0.] * 6)
acc = np.array([0.] * 6)
while t < duration:
# print("############################")
# print(t)
# print(pos)
# print(vel)
# print(acc)
point = JointTrajectoryPoint()
point.positions = pos + init_pose
point.velocities = [0.] * 6
point.accelerations = [0.] * 6
point.time_from_start = rospy.Duration(t)
traj.points.append(point)
pos += vel * time_step
pos = np.minimum(pos, joint_limits[:, 1])
pos = np.maximum(pos, joint_limits[:, 0])
vel += acc * time_step
vel = np.minimum(vel, vel_limit)
vel = np.maximum(vel, -vel_limit)
# print (iters % action_step)
if iters % action_step == 0:
acc = np.random.rand(6) - 0.5
acc = acc / np.linalg.norm(acc)
acc *= acc_mod
t += time_step
iters += 1
point = JointTrajectoryPoint()
point.positions = init_pose
point.velocities = [0.] * 6
point.accelerations = [0.] * 6
point.time_from_start = rospy.Duration(duration + 3)
traj.points.append(point)
return traj
def create_grasp(self, pose, grasp_id):
"""
:type pose: Pose
pose of the gripper for the grasp
:type grasp_id: str
name for the grasp
:rtype: Grasp
"""
g = Grasp()
g.id = grasp_id
pre_grasp_posture = JointTrajectory()
pre_grasp_posture.header.frame_id = self._grasp_postures_frame_id
pre_grasp_posture.joint_names = [
name for name in self._gripper_joint_names.split()
]
jtpoint = JointTrajectoryPoint()
jtpoint.positions = [
float(pos) for pos in self._gripper_pre_grasp_positions.split()
]
jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
pre_grasp_posture.points.append(jtpoint)
grasp_posture = copy.deepcopy(pre_grasp_posture)
grasp_posture.points[0].time_from_start = rospy.Duration(
self._time_pre_grasp_posture + self._time_grasp_posture)
jtpoint2 = JointTrajectoryPoint()
jtpoint2.positions = [
float(pos) for pos in self._gripper_grasp_positions.split()
]
jtpoint2.time_from_start = rospy.Duration(
self._time_pre_grasp_posture + self._time_grasp_posture +
self._time_grasp_posture_final)
grasp_posture.points.append(jtpoint2)
g.pre_grasp_posture = pre_grasp_posture
g.grasp_posture = grasp_posture
header = Header()
header.frame_id = self._grasp_pose_frame_id # base_footprint
q = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
# Fix orientation from gripper_link to parent_link (tool_link)
fix_tool_to_gripper_rotation_q = quaternion_from_euler(
math.radians(self._fix_tool_frame_to_grasping_frame_roll),
math.radians(self._fix_tool_frame_to_grasping_frame_pitch),
math.radians(self._fix_tool_frame_to_grasping_frame_yaw))
q = quaternion_multiply(q, fix_tool_to_gripper_rotation_q)
fixed_pose = copy.deepcopy(pose)
fixed_pose.orientation = Quaternion(*q)
g.grasp_pose = PoseStamped(header, fixed_pose)
g.grasp_quality = self._grasp_quality
g.pre_grasp_approach = GripperTranslation()
g.pre_grasp_approach.direction.vector.x = self._pre_grasp_direction_x # NOQA
g.pre_grasp_approach.direction.vector.y = self._pre_grasp_direction_y # NOQA
g.pre_grasp_approach.direction.vector.z = self._pre_grasp_direction_z # NOQA
g.pre_grasp_approach.direction.header.frame_id = self._grasp_postures_frame_id # NOQA
g.pre_grasp_approach.desired_distance = self._grasp_desired_distance # NOQA
g.pre_grasp_approach.min_distance = self._grasp_min_distance
g.post_grasp_retreat = GripperTranslation()
g.post_grasp_retreat.direction.vector.x = self._post_grasp_direction_x # NOQA
g.post_grasp_retreat.direction.vector.y = self._post_grasp_direction_y # NOQA
g.post_grasp_retreat.direction.vector.z = self._post_grasp_direction_z # NOQA
g.post_grasp_retreat.direction.header.frame_id = self._grasp_postures_frame_id # NOQA
g.post_grasp_retreat.desired_distance = self._grasp_desired_distance # NOQA
g.post_grasp_retreat.min_distance = self._grasp_min_distance
g.max_contact_force = self._max_contact_force
g.allowed_touch_objects = self._allowed_touch_objects
return g
def add_point(self, positions, goal, time):
# creates a point in trajectory with time_from_start and positions
point = JointTrajectoryPoint()
point.positions = copy.copy(positions)
point.time_from_start = rospy.Duration(time)
goal.trajectory.points.append(point)
def calculate_sensor_offsets():
rospy.init_node('calculate_offsets')
package_to_store = rospy.get_param('~package_to_store')
store_to_file = rospy.get_param('~store_to_file')
scenario = rospy.get_param('~scenario')
robot = rospy.get_param('~robot')
set_calculated_offset = rospy.get_param('~set_calculated_offset')
offset_params_ns = rospy.get_param('~offset_params_ns', "/temp") # /arm/driver
recalibrate_srv_ns = rospy.get_param('~recalibrate_srv_ns', "") # /arm/driver
if robot == "kuka":
joint_names = rospy.get_param('/controller_joint_names')
controller_topic = '/position_trajectory_controller/command'
calcOffset_service = '/CalculateOffsets'
setOffset_service = '/SetSensorOffset'
elif robot == "ur":
joint_names = rospy.get_param("/hardware_interface/joints")
controller_topic = "/pos_based_pos_traj_controller/command"
calcOffset_service = '/CalculateOffsets'
setOffset_service = '/SetSensorOffset'
else:
joint_names = rospy.get_param('/arm/joint_names')
controller_topic = '/arm/joint_trajectory_controller/command'
calcOffset_service = '/arm/CalculateOffsets'
setOffset_service = '/arm/SetSensorOffset'
trajectory_pub = rospy.Publisher(controller_topic, JointTrajectory, latch=True, queue_size=1)
calculate_offsets_srv = rospy.ServiceProxy(calcOffset_service, CalculateSensorOffset)
set_offsets_srv = rospy.ServiceProxy(setOffset_service, SetSensorOffset)
##print call('rospack find force_torque_sensor', shell=True)
##call('rosparam dump -v `rospack find force_torque_sensor`/config/sensor_offset.yaml /fts/Offset')
# Posees
poses = [[0.0, 0.0, 1.5707963, 0.0, -1.5707963, 0.0],
[0.0, 0.0, 1.5707963, 0.0, 1.5707963, 0.0]]
poses_kuka = [[0.0, -1.5707963, 1.5707963, 0.0, -1.5707963, 0.0],
[0.0, -1.5707963, 1.5707963, 0.0, 1.5707963, 0.0]]
poses_ur = [[1.5707963, -1.5707963, 1.5707963, -1.5707963, 1.5707963, 0.0],
[1.5707963, -1.5707963, 1.5707963, -1.5707963, -1.5707963, 0.0]]
measurement = Wrench()
for i in range(0,len(poses)):
trajectory = JointTrajectory()
point = JointTrajectoryPoint()
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = joint_names
point.time_from_start = rospy.Duration(2.5)
if robot == "kuka":
point.positions = poses_kuka[i]
elif robot == "ur":
point.positions = poses_ur[i]
else:
point.positions = poses[i]
trajectory.points.append(point)
trajectory_pub.publish(trajectory)
rospy.loginfo("Going to position: " + str(point.positions))
rospy.sleep(10.0)
rospy.loginfo("Calculating offsets.")
ret = calculate_offsets_srv(False)
measurement.force.x += ret.offset.force.x
measurement.force.y += ret.offset.force.y
measurement.force.z += ret.offset.force.z
measurement.torque.x += ret.offset.torque.x
measurement.torque.y += ret.offset.torque.y
measurement.torque.z += ret.offset.torque.z
measurement.force.x /= len(poses)
measurement.force.y /= len(poses)
measurement.force.z /= len(poses)
measurement.torque.x /= len(poses)
measurement.torque.y /= len(poses)
measurement.torque.z /= len(poses)
rospy.set_param(offset_params_ns + '/Offset/force/x', measurement.force.x)
rospy.set_param(offset_params_ns + '/Offset/force/y', measurement.force.y)
rospy.set_param(offset_params_ns + '/Offset/force/z', measurement.force.z)
rospy.set_param(offset_params_ns + '/Offset/torque/x', measurement.torque.x)
rospy.set_param(offset_params_ns + '/Offset/torque/y', measurement.torque.y)
rospy.set_param(offset_params_ns + '/Offset/torque/z', measurement.torque.z)
if set_calculated_offset:
ret = set_offsets_srv(measurement)
rospy.loginfo(ret.message)
#client = dynamic_reconfigure.client.Client(recalibrate_srv_ns)
#client.update_configuration({"force":{"x":measurement.force.x, "y":measurement.force.y, "z":measurement.force.z}, "torque":{"x":measurement.torque.x, "y":measurement.torque.y, "z":measurement.torque.z}})
if store_to_file:
if scenario == '':
call('rosparam dump -v `rospack find ' + package_to_store + ' `/config/sensor_offset.yaml ' + offset_params_ns + '/Offset', shell=True)
else:
call('rosparam dump -v `rospack find ' + package_to_store + ' `/config/robot_with_' + scenario + '_offset.yaml ' + offset_params_ns + '/Offset', shell=True)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
a0, a1, a2, a3, a4, a5, a6 =symbols('a0:7') #link length
d1, d2, d3, d4, d5, d6, d7 =symbols('d1:8') #link offsets
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 =symbols('alpha0:7') #twist angles
#create symbols for theta
q1, q2, q3, q4, q5, q6, q7 =symbols('q1:8') #thetas
# Create Modified DH parameters
s={ alpha0: 0, a0: 0, d1: 0.75, q1: q1,
alpha1: -pi/2, a1: 0.35, d2: 0, q2: -pi/2+q2,
alpha2: 0, a2: 1.25, d3: 0, q3: q3,
alpha3: -pi/2, a3: -0.054, d4: 1.5, q4: q4,
alpha4: pi/2, a4: 0, d5: 0, q5: q5,
alpha5: -pi/2, a5: 0, d6: 0, q5: q6,
alpha6: 0, a6: 0, d7: 0.303, q7: 0}
# Define Modified DH Transformation matrix
def TF_Matrix(alpha, a, d, q):
TF = Matrix([[ cos(q), -sin(q), 0, a],
[ sin(q)*cos(alpha), cos(q)*cos(alpha), -sin(alpha), -sin(alpha)*d],
[ sin(q)*sin(alpha), cos(q)*sin(alpha), cos(alpha), cos(alpha)*d],
[ 0, 0, 0, 1]])
return TF
# Create individual transformation matrices
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(s)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(s)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(s)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(s)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(s)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(s)
T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(s)
T0_EE= T0_1*T1_2*T2_3*T3_4*T4_5*T5_6*T6_EE #from base length to end effector
#
# Extract rotation matrices from the transformation matrices
#
#
###
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
#
#
# Calculate joint angles using Geometric IK method
#
## Extract rotation matrices from the transformation matrices
#
#
r, p, y = symbols('r p y')
R_x = Matrix([[1, 0, 0],
[0, cos(r), -sin(r)],
[0 ,sin(r), cos(r)]])
R_y = Matrix([[cos(p), 0, sin(p)],
[0, 1, 0],
[-sin(p), 0, cos(p)]])
R_z = Matrix([[cos(y), -sin(y), 0],
[sin(y), cos(y), 0],
[0 , 0, 1]])
R_EE = R_z * R_y * R_x #extrinsic rotation
R_correction = R_z.subs(y, pi) * R_y.subs(p, -pi/2)
R_EE = R_EE* R_correction
R_EE = R_EE. subs({'r': roll, 'p' :pitch, 'y': yaw})
#postion of Wrist circle
EE = Matrix([[px],
[py],
[pz]])
WC = EE - (0.303)*R_EE[:,2]
a = 1.501
b = sqrt(pow((sqrt(WC[0]*WC[0] + WC[1] * WC[1])-0.35),2)+pow((WC[2]-0.75),2))
c = 1.25
theta1 = atan2(WC[1],WC[0])
theta1_2 = acos((b*b + c*c - a*a)/(2*b*c))
theta2_3 = acos((a*a + c*c - b*b)/(2*a*c))
theta2 = (pi/2 - theta1_2 - atan2(WC[2] - 0.75, sqrt(WC[0]*WC[0] + WC[1] * WC[1]) - 0.35))
theta3 = (pi/2 - (theta2_3 + 0.036))
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1:theta1, q2:theta2, q3:theta3})
R3_6 = R0_3.inv("LU") * R_EE
theta4 = atan2(R3_6[2,2], -R3_6[0,2])
theta6 = atan2(-R3_6[1,1], R3_6[1,0])
theta5 = atan2(sqrt(pow(R3_6[0,2],2)+pow(R3_6[2,2],2)), R3_6[1,2])
###
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def main(whole_body, base, gripper, wrist_wrench):
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
# publisher for delvering command for base move
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
# base_pub = rospy.Publisher('/move_base/move/goal',move_base_msgs.msg.MoveBaseActionGoal,queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
## Grab the handle of door
#test with service - get the handle position from handle
grasp_point_client()
global recog_pos
global Is_found
print recog_pos.pose.position
# target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point", PoseStamped)
# recog_pos.pose.position.x=target_pose_Msg.pose.position.x
# recog_pos.pose.position.y=target_pose_Msg.pose.position.y
# recog_pos.pose.position.z=target_pose_Msg.pose.position.z
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_X,
y=recog_pos.pose.position.y - HANDLE_TO_HAND_POS_Y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
#change the impedance config to grasping
whole_body.impedance_config = 'grasping'
## Direct Joint trajectory
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.07
current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(3.0)
## Move by End-effector line
whole_body.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
## Move base with linear & Angular motion
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
## Move base with linear & Angular motion second
tw_cmd0 = geometry_msgs.msg.Twist()
tw_cmd0.linear.x = 0.3
tw_cmd0.angular.z = 0.45
vel_pub.publish(tw_cmd0)
# rospy.sleep(4.0)
rospy.sleep(4.0)
## Open the gripper
gripper.command(1.0)
## Move back
tw_cmd = geometry_msgs.msg.Twist()
tw_cmd.linear.x = -1.2
vel_pub.publish(tw_cmd)
rospy.sleep(2.0)
## Move back 2
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -1.1
tw_cmd2.angular.z = -0.4
vel_pub.publish(tw_cmd2)
rospy.sleep(4.0)
whole_body.move_to_neutral()
## Move back
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -0.6
tw_cmd2.angular.z = -0.3
vel_pub.publish(tw_cmd2)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# Create symbols
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Modified DH parameters
s = {
a0: 0,
alpha0: 0,
d1: 0.75,
a1: 0.35,
alpha1: -pi / 2,
d2: 0,
q2: q2 - pi / 2,
a2: 1.25,
alpha2: 0,
d3: 0,
a3: -0.054,
alpha3: -pi / 2,
d4: 1.5,
a4: 0,
alpha4: pi / 2,
d5: 0,
a5: 0,
alpha5: -pi / 2,
d6: 0,
a6: 0,
alpha6: 0,
d7: 0.303,
q7: 0
}
#
#
#Modified DH Transformation matrix
T0_1 = Matrix([[cos(q1), -sin(q1), 0, a0],
[
sin(q1) * cos(alpha0),
cos(q1) * cos(alpha0), -sin(alpha0),
-sin(alpha0) * d1
],
[
sin(q1) * sin(alpha0),
cos(q1) * sin(alpha0),
cos(alpha0),
cos(alpha0) * d1
], [0, 0, 0, 1]])
T0_1 = T0_1.subs(s)
T1_2 = Matrix([[cos(q2), -sin(q2), 0, a1],
[
sin(q2) * cos(alpha1),
cos(q2) * cos(alpha1), -sin(alpha1),
-sin(alpha1) * d2
],
[
sin(q2) * sin(alpha1),
cos(q2) * sin(alpha1),
cos(alpha1),
cos(alpha1) * d2
], [0, 0, 0, 1]])
T1_2 = T1_2.subs(s)
T2_3 = Matrix([[cos(q3), -sin(q3), 0, a2],
[
sin(q3) * cos(alpha2),
cos(q3) * cos(alpha2), -sin(alpha2),
-sin(alpha2) * d3
],
[
sin(q3) * sin(alpha2),
cos(q3) * sin(alpha2),
cos(alpha2),
cos(alpha2) * d3
], [0, 0, 0, 1]])
T2_3 = T2_3.subs(s)
T3_4 = Matrix([[cos(q4), -sin(q4), 0, a3],
[
sin(q4) * cos(alpha3),
cos(q4) * cos(alpha3), -sin(alpha3),
-sin(alpha3) * d4
],
[
sin(q4) * sin(alpha3),
cos(q4) * sin(alpha3),
cos(alpha3),
cos(alpha3) * d4
], [0, 0, 0, 1]])
T3_4 = T3_4.subs(s)
T4_5 = Matrix([[cos(q5), -sin(q5), 0, a4],
[
sin(q5) * cos(alpha4),
cos(q5) * cos(alpha4), -sin(alpha4),
-sin(alpha4) * d5
],
[
sin(q5) * sin(alpha4),
cos(q5) * sin(alpha4),
cos(alpha4),
cos(alpha4) * d5
], [0, 0, 0, 1]])
T4_5 = T4_5.subs(s)
T5_6 = Matrix([[cos(q6), -sin(q6), 0, a5],
[
sin(q6) * cos(alpha5),
cos(q6) * cos(alpha5), -sin(alpha5),
-sin(alpha5) * d6
],
[
sin(q6) * sin(alpha5),
cos(q6) * sin(alpha5),
cos(alpha5),
cos(alpha5) * d6
], [0, 0, 0, 1]])
T5_6 = T5_6.subs(s)
T6_7 = Matrix([[cos(q7), -sin(q7), 0, a6],
[
sin(q7) * cos(alpha6),
cos(q7) * cos(alpha6), -sin(alpha6),
-sin(alpha6) * d7
],
[
sin(q7) * sin(alpha6),
cos(q7) * sin(alpha6),
cos(alpha6),
cos(alpha6) * d7
], [0, 0, 0, 1]])
T6_7 = T6_7.subs(s)
# Create individual transformation matrices
T0_2 = simplify(T0_1 * T1_2)
T0_3 = simplify(T0_2 * T2_3)
T0_4 = simplify(T0_3 * T3_4)
T0_5 = simplify(T0_4 * T4_5)
T0_6 = simplify(T0_5 * T5_6)
T0_7 = simplify(T0_6 * T6_7)
#Rotation Matrix
x_angle, y_angle, z_angle = symbols('x_angle y_angle z_angel')
R_Z = Matrix([[cos(z_angle), -sin(z_angle), 0, 0],
[sin(z_angle), cos(z_angle), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
R_Y = Matrix([[cos(y_angle), 0, sin(y_angle), 0], [0, 1, 0, 0],
[-sin(y_angle), 0, cos(y_angle), 0], [0, 0, 0, 1]])
R_X = Matrix([[1, 0, 0, 0], [0, cos(x_angle), -sin(x_angle), 0],
[0, sin(x_angle), cos(x_angle), 0], [0, 0, 0, 1]])
#Correction matrix, This matrix is you to transform Gazebo coordinate system to DH coordinate system
R_corr = simplify(R_Z * R_Y)
T_total = simplify(T0_7 * R_corr)
#
#
# Extract rotation matrices from the transformation matrices
R0_1 = T0_1[:3, :3]
R0_2 = T0_2[:3, :3]
R0_3 = T0_3[:3, :3]
R0_4 = T0_4[:3, :3]
R0_5 = T0_5[:3, :3]
R0_6 = T0_6[:3, :3]
R0_7 = T0_7[:3, :3]
#
#
###
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
# Compensate for rotation discrepancy between DH parameters and Gazebo
# r_corr is the correction matrix require to transform from gazebo coordinate system to DH coordinate system
r_corr = R_corr[:3, :3]
r_corr = r_corr.evalf(subs={y_angle: -pi / 2, z_angle: pi})
r_x = R_X[:3, :3]
r_y = R_Y[:3, :3]
r_z = R_Z[:3.:3]
# Rotation matrix for end effector (In Gazebo coordinate systme)
Rrpy = simplify(r_z * r_y * r_x)
R_EE_URDF = Rrpy.evalf(subs={
x_angel: roll,
y_angle: pitch,
z_angle: yaw
})
# Rotation matrix for end effector (In DH coordinate system)
R_EE_DH = R_EE_URDF * r_corr
# Wrist center position
WC_pos = Matrix([[px], [py], [pz]]) - d7 * R_EE_DH[:, 2]
# Calculate joint angles using Geometric IK method
# Look the the triangle abc image in writup
# A,B and C are the sides of triangle
theta1 = atan2(WC_pos[1], WC_pos[0])
A = 1.5009
C = 1.25
B = (((WC_pos[0]**2 + WC_pos[1]**2)**(0.5) - 0.35)**2 +
(WC_pos[2] - 0.75)**2)**(
0.5) #0.35 is a1, 0.75 is the z-coordinate of joint 2
# Standered cos rule to find the angles of triangle
a_angle = acos((-A**2 + B**2 + C**2) / (2 * B * C))
b_angle = acos((A**2 - B**2 + C**2) / (2 * A * C))
c_angle = acos((A**2 + B**2 - C**2) / (2 * B * A))
theta2 = pi / 2 - a_angle - atan2(
WC_pos[2] - 0.75, (WC_pos[0]**2 + WC_pos[1]**2)**(0.5) - 0.35)
theta3 = (
pi / 2 - 0.036
) - b_angle # pi/2 - 0.036 is the initial angle between the side A and side B (look the dig1 in readme file)
R0_3_val = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
#R3_6 is the rotation matrix for 3rd joint to 6th joint coordinate system
R3_6 = R0_3_val.inv('LU') * R_EE_DH
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2((R3_6[0, 2]**2 + R3_6[2, 2]**2)**0.5, R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
#
###
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
#Calculating the error in the position of end-effector
EE = T_total.evalf(
subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
EE_pos_cal = [EE[0, 3], EE[1, 3], EE[2, 3]]
ee_x_e = abs(EE_pos_cal[0] - px)
ee_y_e = abs(EE_pos_cal[1] - py)
ee_z_e = abs(EE_pos_cal[2] - pz)
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" %
ee_offset)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def move_arm( positions, time_from_start = 3.0, arm = 'r', client = None ):
if (client == None):
client = init_jt_client(arm)
else:
arm = client.action_client.ns[0:1]; # ignore arm argument
rospy.loginfo( 'move arm \'%s\' to position %s'%( arm, positions ) )
joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) )
jt = JointTrajectory()
jt.joint_names = joint_names
jt.points = []
jp = JointTrajectoryPoint()
jp.time_from_start = rospy.Time.from_seconds( time_from_start )
jp.positions = positions
jt.points.append( jp )
# push trajectory goal to ActionServer
jt_goal = JointTrajectoryGoal()
jt_goal.trajectory = jt
jt_goal.trajectory.header.stamp = rospy.Time.now() # + rospy.Duration.from_sec( time_from_start )
client.send_goal( jt_goal )
client.wait_for_result()
return client.get_state()
