def interpolate_joint_space(self, goal, total_time, nb_points, start=None):
"""
Interpolate a trajectory from a start state (or current state) to a goal in joint space
:param goal: A RobotState to be used as the goal of the trajectory
param total_time: The time to execute the trajectory
:param nb_points: Number of joint-space points in the final trajectory
:param start: A RobotState to be used as the start state, joint order must be the same as the goal
:return: The corresponding RobotTrajectory
"""
dt = total_time*(1.0/nb_points)
# create the joint trajectory message
traj_msg = JointTrajectory()
rt = RobotTrajectory()
if start == None:
start = self.get_current_state()
joints = []
start_state = start.joint_state.position
goal_state = goal.joint_state.position
for j in range(len(goal_state)):
pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1)
joints.append(pose_lin[1:].tolist())
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
traj_msg.points.append(point)
# put name of joints to be moved
traj_msg.joint_names = self.joint_names()
# send the message
rt.joint_trajectory = traj_msg
return rt
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 createHeadGoal(self):
(roll, pitch, yaw) = euler_from_quaternion([self.last_oculus_msg.pose.orientation.x,
self.last_oculus_msg.pose.orientation.y,
self.last_oculus_msg.pose.orientation.z,
self.last_oculus_msg.pose.orientation.w])
rospy.loginfo("roll, pitch, yaw: ")
rospy.loginfo(str(roll) + " " + str(pitch) + " " + str(yaw))
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory.joint_names.append('head_1_joint') # 1 positivo izquierda, -1 derecha
head_goal.trajectory.joint_names.append('head_2_joint') # -1 arriba, 1 abajo
# Current position trajectory point
jtp_current = JointTrajectoryPoint()
jtp_current.positions.append(self.last_head_state.actual.positions[0])
jtp_current.positions.append(self.last_head_state.actual.positions[1])
jtp_current.velocities.append(self.last_head_state.actual.velocities[0])
jtp_current.velocities.append(self.last_head_state.actual.velocities[1])
jtp_current.time_from_start = rospy.Duration(0.0)
# Next goal position
jtp = JointTrajectoryPoint()
jtp.positions.append(yaw *-1)
jtp.positions.append(pitch *-1)
# jtp.positions.append((yaw *-1) + 1.5707963267948966) # + 180 deg
# jtp.positions.append((roll + 1.5707963267948966) *-1) # + 180 deg because of torso2
jtp.velocities.append(0.0)
jtp.velocities.append(0.0)
rospy.loginfo("Sending: " + str(jtp.positions))
#jtp.time_from_start.secs = 1
jtp.time_from_start.nsecs = 100
head_goal.trajectory.points.append(jtp_current)
head_goal.trajectory.points.append(jtp)
head_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
return head_goal
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 convert_trajectory(self, traj):
""" Converts a trajectory into a joint trajectory
Args:
traj: Trajectory to convert
Returns:
joint trajectory
"""
new_traj = JointTrajectory()
new_traj.header = traj.header
# Take each joint in the trajectory and add it to the joint trajectory
for joint_name in traj.joint_names:
new_traj.joint_names.append(self.joint_map[joint_name].joint_name)
# For each poiint in the trajectory
for point in traj.points:
new_point = JointTrajectoryPoint()
for i, pos in enumerate(point.positions):
new_point.positions.append(self.joint_map[new_traj.joint_names[i]].convert_angle(pos))
for i, vel in enumerate(point.velocities):
new_point.velocities.append(self.joint_map[new_traj.joint_names[i]].convert_velocity(vel))
new_point.time_from_start = point.time_from_start
new_traj.points.append(new_point)
return new_traj
def add_point(self, positions, velocities, accelerations, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = copy(velocities)
point.accelerations = copy(accelerations)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
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 test_5(self):
# Test 5: move each joint in robotic arm to a specific value at the same time (mixed speeds)
rospy.loginfo("Begin test 5")
# 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 stowed 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, -1.3, 1.7, 1.2, 0.1]
point.velocities = [90.0, 50.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.loginfo("End test 5")
def createHandGoal(side, j1, j2, j3):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
with the hand specified in side
@arg side string 'right' or 'left'
@arg j1 float value for joint 'hand_'+side+'_thumb_joint'
@arg j2 float value for joint 'hand_'+side+'_middle_joint'
@arg j3 float value for joint 'hand_'+side+'_index_joint'
@return FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('hand_'+side+'_thumb_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_middle_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_index_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.positions.append(j3)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
fjtg.trajectory.points.append(point)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
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 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 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 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 createHeadGoal(j1, j2):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1 and j2 for the joint positions
@arg j1 float value for head_1_joint
@arg j2 float value for head_2_joint
@returns FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('head_1_joint')
fjtg.trajectory.joint_names.append('head_2_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.points.append(point)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
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 joint_trajectory(self, joint_trajectory):
'''
Returns just the part of the trajectory corresponding to the arm.
**Args:**
**joint_trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to convert
**Returns:**
A trajectory_msgs.msg.JointTrajectory corresponding to only this arm in joint_trajectory
**Raises:**
**ValueError:** If some arm joint is not found in joint_trajectory
'''
arm_trajectory = JointTrajectory()
arm_trajectory.header = copy.deepcopy(joint_trajectory.header)
arm_trajectory.joint_names = copy.copy(self.joint_names)
indexes = [-1]*len(self.joint_names)
for (i, an) in enumerate(self.joint_names):
for (j, n) in enumerate(joint_trajectory.joint_names):
if an == n:
indexes[i] = j
break
if indexes[i] < 0:
raise ValueError('Joint '+n+' is missing from joint trajectory')
for joint_point in joint_trajectory.points:
arm_point = JointTrajectoryPoint()
arm_point.time_from_start = joint_point.time_from_start
for i in indexes:
arm_point.positions.append(joint_point.positions[i])
arm_trajectory.points.append(arm_point)
return arm_trajectory
def trajectory_cb(self, traj):
rospy.loginfo("Received a new trajectory, beginning the split..")
newtraj_arm = JointTrajectory()
newtraj_arm.header.stamp = traj.header.stamp
newtraj_arm.header.frame_id = traj.header.frame_id
for name in traj.joint_names:
if name[:5] == "Joint":
newtraj_arm.joint_names += (name,)
for point in traj.points:
newpoint = JointTrajectoryPoint()
for i, name in enumerate(traj.joint_names):
if name[:5] == "Joint":
newpoint.positions += (point.positions[i],)
newpoint.velocities += (point.velocities[i],)
newpoint.accelerations += (point.accelerations[i],)
newpoint.time_from_start = point.time_from_start
newtraj_arm.points += (newpoint,)
# rospy.loginfo("Sending trajectory:\n" + str(newtraj_arm))
rospy.loginfo("Publishing the arm trajectory")
self.traj_publisher.publish(newtraj_arm)
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 make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id="/base_link"):
"""
This function will convert the plan to a joint trajectory compatible with the
/arm_N/arm_controller/command topic
"""
theplan = plan
# create joint trajectory message
jt = JointTrajectory()
# fill the header
jt.header.seq = seq
jt.header.stamp.secs = 0 # secs
jt.header.stamp.nsecs = 0 # nsecs
jt.header.frame_id = frame_id
# specify the joint names
jt.joint_names = theplan.joint_trajectory.joint_names
# fill the trajectory points and computer constraint times
njtp = len(theplan.joint_trajectory.points)
for ii in range(0, njtp):
jtp = JointTrajectoryPoint()
jtp = copy.deepcopy(theplan.joint_trajectory.points[ii])
jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0]
jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0]
jtp.time_from_start = rospy.Duration.from_sec(secs + dt * (ii + 1))
jt.points.append(jtp)
return jt
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 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 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 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 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 _execute_trajectory(self, trajectory):
if self._running:
return
self._running = True
joint_ids = [self._current_joint_states.name.index(joint_name) for joint_name in trajectory.joint_names]
position_previous = JointTrajectoryPoint()
position_previous.positions = [self._current_joint_states.position[joint_id] for joint_id in joint_ids]
# print 'starting from: ' + ('%6.2f ' * len(joint_ids)) % tuple(position_previous.positions)
position_previous.time_from_start = rospy.Duration(0.0)
position_index = 0
start_time = rospy.get_rostime()
while position_index < len(trajectory.points):
now = rospy.get_rostime() - start_time
# print ' to: ' + ('%6.2f ' * len(joint_ids)) % tuple(trajectory.points[position_index].positions)
while now <= trajectory.points[position_index].time_from_start:
delta_time = now - position_previous.time_from_start
position_duration = trajectory.points[position_index].time_from_start - position_previous.time_from_start
if position_duration.to_sec() == 0.0:
progress = 1.0
else:
progress = delta_time.to_sec() / position_duration.to_sec()
for joint_index, goal_position in enumerate(trajectory.points[position_index].positions):
joint_delta = progress * (goal_position - position_previous.positions[joint_index])
joint_position = position_previous.positions[joint_index] + joint_delta
self._joint_states.position[joint_index] = joint_position
self._state_publisher.publish(self._joint_states)
now = rospy.get_rostime() - start_time
rospy.sleep(0.01)
position_previous = trajectory.points[position_index]
position_index += 1
self._running = False
def head_up_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = [0]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(1.0)
jtm.points = [jtp]
return jtm
def send_goal(self, goal):
"""
Send goal to the controller.
:param JointTrajectory goal: Goal to send.
"""
self.goal_done = False
self.last_result = None
self.last_feedback = None
# Check which of our joints the goal has
joints_in_goal = []
for j in goal.joint_names:
if j in self.managed_joints:
joints_in_goal.append(j)
rospy.loginfo("This controller manages the joints: " +
str(self.managed_joints))
rospy.loginfo("Goal contains our joints: " + str(joints_in_goal))
missing_joints = list(set(self.managed_joints) - set(joints_in_goal))
rospy.loginfo("Goal is missing joints: " + str(missing_joints))
extra_joints = list(set(goal.joint_names) - set(joints_in_goal))
rospy.loginfo("Discarding joints not from this controller: " +
str(extra_joints))
if len(extra_joints) > 0:
# Create a new goal only with the found joints, in the next
# step it will be filled with any missing joints
jt = JointTrajectory()
jt.joint_names = joints_in_goal
for jp in goal.points:
p = JointTrajectoryPoint()
p.time_from_start = jp.time_from_start
for jname in joints_in_goal:
idx = goal.joint_names.index(jname)
p.positions.append(jp.positions[idx])
if jp.velocities:
p.velocities.append(jp.velocities[idx])
if jp.accelerations:
p.accelerations.append(jp.accelerations[idx])
if jp.effort:
p.effort.append(jp.effort[idx])
jt.points.append(p)
goal = jt
if len(missing_joints) == len(self.managed_joints):
# This goal contains no joints of this controller
# we don't do anything
rospy.loginfo("Goal contains no joints of this controller.")
return
if len(missing_joints) > 0:
# Fill message for the real controller with the missing joints
# Transform tuple fields into lists
goal.joint_names = list(goal.joint_names)
for point in goal.points: # type: JointTrajectoryPoint
point.positions = list(point.positions)
if point.velocities:
point.velocities = list(point.velocities)
if point.accelerations:
point.accelerations = list(point.accelerations)
if point.effort:
point.effort = list(point.effort)
# Now add the missing joints
for jname in missing_joints:
goal.joint_names.append(jname)
jidx = self.managed_joints.index(jname)
# TODO: remove this if-assert (it's redundant)
if jidx == -1:
rospy.logerror("Couldn't find joint " + jname +
", which we should be controlling," +
" in our managed joints.")
assert False
for point in goal.points:
point.positions.append(
self.last_state.actual.positions[jidx])
if point.velocities:
point.velocities.append(
self.last_state.actual.velocities[jidx])
if self.last_state.actual.accelerations:
point.accelerations.append(
self.last_state.actual.accelerations[jidx])
# TODO: deal with forces, they aren't published in state...
# but they are in joint_states
# Now the trajectory should be complete and we can send the goal
jtg = FollowJointTrajectoryGoal()
jtg.trajectory = goal
rospy.loginfo("Sending goal: " + str(goal))
self._ac.send_goal(jtg,
done_cb=self.done_cb,
feedback_cb=self.feedback_cb)
def getCartesianMove(self,
frame,
q0,
base_steps=1000,
steps_per_meter=1000,
steps_per_radians=4,
time_multiplier=1,
percent_acc=1,
use_joint_move=False,
table_frame=None):
if table_frame is not None:
if frame.p[2] < table_frame[0][2]:
rospy.logerr(
"Ignoring move to waypoint due to relative z: %f < %f" %
(frame.p[2], table_frame[0][2]))
return JointTrajectory()
if q0 is None:
rospy.logerr("Invalid initial joint position in getCartesianMove")
return JointTrajectory()
# interpolate between start and goal
pose = pm.fromMatrix(self.kdl_kin.forward(q0))
cur_rpy = np.array(pose.M.GetRPY())
cur_xyz = np.array(pose.p)
goal_rpy = np.array(frame.M.GetRPY())
goal_xyz = np.array(frame.p)
delta_rpy = np.linalg.norm(goal_rpy - cur_rpy)
delta_translation = (pose.p - frame.p).Norm()
if delta_rpy < 0.001 and delta_translation < 0.001:
rospy.logwarn("Robot is already in the goal position.")
return JointTrajectory(points=[
JointTrajectoryPoint(positions=q0,
velocities=[0] * len(q0),
accelerations=[0] * len(q0),
time_from_start=rospy.Duration(0.0))
],
joint_names=self.joint_names)
q_target = self.ik(pm.toMatrix(frame), q0)
if q_target is None:
rospy.logerr("No IK solution on cartesian move target")
return JointTrajectory()
else:
if np.any(
np.greater(
np.absolute(q_target[:2] - np.array(q0[:2])), np.pi/2)) \
or np.absolute(q_target[3] - q0[3]) > np.pi:
rospy.logerr("Dangerous IK solution, abort getCartesianMove")
return JointTrajectory()
dq_target = q_target - np.array(q0)
if np.sum(np.absolute(dq_target)) < 0.0001:
rospy.logwarn("Robot is already in the goal position.")
return JointTrajectory(points=[
JointTrajectoryPoint(positions=q0,
velocities=[0] * len(q0),
accelerations=[0] * len(q0),
time_from_start=rospy.Duration(0.0))
],
joint_names=self.joint_names)
steps, t_v_const_step, t_v_setting_max, steps_to_max_speed, const_velocity_max_step = self.calculateAccelerationProfileParameters(
dq_target, base_steps, steps_per_meter, steps_per_radians,
delta_translation, time_multiplier,
self.acceleration_magnification * percent_acc)
traj = JointTrajectory()
traj.points.append(
JointTrajectoryPoint(positions=q0,
velocities=[0] * len(q0),
accelerations=[0] * len(q0)))
# Compute a smooth trajectory.
for i in range(1, steps + 1):
xyz = None
rpy = None
q = None
if not use_joint_move:
xyz = cur_xyz + ((float(i) / steps) * (goal_xyz - cur_xyz))
rpy = cur_rpy + ((float(i) / steps) * (goal_rpy - cur_rpy))
# Create transform for goal frame
frame = pm.toMatrix(
kdl.Frame(kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2]),
kdl.Vector(xyz[0], xyz[1], xyz[2])))
# Use current inverse kinematics solver with current position
q = self.ik(frame, q0)
else:
q = np.array(q0) + (float(i) / steps) * dq_target
q = q.tolist()
#q = self.kdl_kin.inverse(frame,q0)
if self.verbose:
print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q))
if q is not None:
total_time = self.calculateTimeOfTrajectoryStep(
i, steps_to_max_speed, const_velocity_max_step,
t_v_const_step, t_v_setting_max)
# Compute the distance to the last point for each joint. We use this to compute our joint velocities.
dq_i = np.array(q) - np.array(traj.points[-1].positions)
if np.sum(np.abs(dq_i)) < self.skip_tol:
rospy.logwarn(
"Joint trajectory point %d is repeating previous trajectory point. "
% i)
continue
traj.points[i - 1].velocities = (dq_i) / (
total_time - traj.points[i - 1].time_from_start.to_sec())
pt = JointTrajectoryPoint(positions=q,
velocities=[0] * len(q),
accelerations=[0] * len(q))
pt.time_from_start = rospy.Duration(total_time)
# pt.time_from_start = rospy.Duration(i * ts)
traj.points.append(pt)
else:
rospy.logwarn(
"No IK solution on one of the trajectory point to cartesian move target"
)
if len(traj.points) < base_steps:
print rospy.logerr("Planning failure with " \
+ str(len(traj.points)) \
+ " / " + str(base_steps) \
+ " points.")
return JointTrajectory()
traj.joint_names = self.joint_names
return traj
def getJointMove(self,
q_goal,
q0,
base_steps=1000,
steps_per_meter=1000,
steps_per_radians=4,
time_multiplier=1,
percent_acc=1,
use_joint_move=False,
table_frame=None):
if q0 is None:
rospy.logerr("Invalid initial joint position in getJointMove")
return JointTrajectory()
elif np.all(np.isclose(q0, q_goal, atol=0.0001)):
rospy.logwarn("Robot is already in the goal position.")
return JointTrajectory()
if np.any(np.greater(np.absolute(q_goal[:2] - np.array(q0[:2])), np.pi/2)) \
or np.absolute(q_goal[3] - q0[3]) > np.pi:
# TODO: these thresholds should not be set manually here.
rospy.logerr("Dangerous IK solution, abort getJointMove")
return JointTrajectory()
delta_q = np.array(q_goal) - np.array(q0)
# steps = base_steps + int(np.sum(np.absolute(delta_q)) * steps_per_radians)
steps, t_v_const_step, t_v_setting_max, steps_to_max_speed, const_velocity_max_step = self.calculateAccelerationProfileParameters(
delta_q, base_steps, 0, steps_per_radians, 0, time_multiplier,
self.acceleration_magnification * percent_acc)
traj = JointTrajectory()
traj.points.append(
JointTrajectoryPoint(positions=q0,
velocities=[0] * len(q0),
accelerations=[0] * len(q0)))
# compute IK
for i in range(1, steps + 1):
xyz = None
rpy = None
q = None
q = np.array(q0) + (float(i) / steps) * delta_q
q = q.tolist()
if self.verbose:
print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q))
if q is not None:
dq_i = np.array(q) - np.array(traj.points[i - 1].positions)
if np.sum(dq_i) < 0.0001:
rospy.logwarn(
"Joint trajectory point %d is repeating previous trajectory point. "
% i)
# continue
total_time = total_time = self.calculateTimeOfTrajectoryStep(
i, steps_to_max_speed, const_velocity_max_step,
t_v_const_step, t_v_setting_max)
traj.points[i - 1].velocities = dq_i / (
total_time - traj.points[i - 1].time_from_start.to_sec())
pt = JointTrajectoryPoint(positions=q,
velocities=[0] * len(q),
accelerations=[0] * len(q))
pt.time_from_start = rospy.Duration(total_time)
traj.points.append(pt)
else:
rospy.logwarn(
"No IK solution on one of the trajectory point to cartesian move target"
)
if len(traj.points) < base_steps:
print rospy.logerr("Planning failure with " \
+ str(len(traj.points)) \
+ " / " + str(base_steps) \
+ " points.")
return JointTrajectory()
traj.joint_names = self.joint_names
return traj
'right_rh_p12_rn', 'right_rh_r2', 'right_rh_l1', 'right_rh_l2'
]
# Connect to the right arm trajectory action server
print "Connecting to the right gripper trajectory action server..."
gripper_client = actionlib.SimpleActionClient(
'/dual_ur5_arm/right_gripper_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
gripper_client.wait_for_server()
print "Connected"
# Create an arm trajectory with the arm_goal as the end-point
print "Setting up a goal trajectory..."
gripper_trajectory = JointTrajectory()
gripper_trajectory.joint_names = gripper_joints
gripper_trajectory.points.append(JointTrajectoryPoint())
ang = 0.2 # upper is 1.1 # offset from ground is 0.142 when ang=0.72
gripper_trajectory.points[0].positions = [ang, ang / 1.1, ang, ang / 1.1]
gripper_trajectory.points[0].velocities = [0.0 for i in gripper_joints]
gripper_trajectory.points[0].accelerations = [0.0 for i in gripper_joints]
gripper_trajectory.points[0].time_from_start = rospy.Duration(0.5)
# Create an empty trajectory goal
gripper_goal = FollowJointTrajectoryGoal()
# set the trajectory component to the goal trajectory
gripper_goal.trajectory = gripper_trajectory
# specify zero tolerance for the execution time
gripper_goal.goal_time_tolerance = rospy.Duration(0.0)
def close_gripper(self):
self.gripper_msg.points = [JointTrajectoryPoint(positions=[0.274], velocities=[0], time_from_start=rospy.Duration(0.1))]
self.griper_pos.publish(self.gripper_msg)
def executeTraj(trajdata):
#torsogoal = SingleJointPositionGoal()
torso_goal = JointTrajectoryGoal()
arm_goal = JointTrajectoryGoal()
# parse trajectory data into the ROS structure
tokens = trajdata.split()
numpoints = int(tokens[0])
dof = int(tokens[1])
trajoptions = int(tokens[2])
numvalues = dof
offset = 0
if trajoptions & 4: # timestamps
numvalues += 1
offset += 1
if trajoptions & 8: # base transforms
numvalues += 7
if trajoptions & 16: # velocities
numvalues += dof
if trajoptions & 32: # torques
numvalues += dof
for i in range(numpoints):
start = 3 + numvalues * i
torso_pt = JointTrajectoryPoint()
arm_pt = JointTrajectoryPoint()
k = 0
for dofInd in armDofIndices:
pos = float(tokens[start + offset + dofInd])
if jointNameByIndex[dofInd] == 'torso_lift_joint':
if i > 0:
prevpos = torso_goal.trajectory.points[-1].positions[0]
newpos = prevpos + angleDistance(pos, prevpos)
torso_pt.positions.append(newpos)
else:
torso_pt.positions.append(pos)
else:
if i > 0:
prevpos = arm_goal.trajectory.points[-1].positions[k]
newpos = prevpos + angleDistance(pos, prevpos)
arm_pt.positions.append(newpos)
else:
arm_pt.positions.append(pos)
k = k + 1
if trajoptions & 4:
arm_pt.time_from_start = rospy.Duration(float(tokens[start]))
torso_pt.time_from_start = rospy.Duration(float(tokens[start]))
arm_goal.trajectory.joint_names = \
filter(lambda jn: jn!='torso_lift_joint',
[jointNameByIndex[i] for i in armDofIndices])
arm_goal.trajectory.points.append(arm_pt)
torso_goal.trajectory.joint_names = ['torso_lift_joint']
torso_goal.trajectory.points.append(torso_pt)
print 'sending arm JointTrajectoryGoal'
arm_joint_action_client.send_goal(arm_goal)
print 'sending torso JointTrajectoryGoal'
torso_joint_action_client.send_goal(torso_goal)
arm_joint_action_client.wait_for_result()
torso_joint_action_client.wait_for_result()
arm_state = arm_joint_action_client.get_state()
torso_state = torso_joint_action_client.get_state()
return arm_state, torso_state
rospy.loginfo("Turn on motors")
rospy.wait_for_service('/ve026a/set_motor')
try:
res = rospy.ServiceProxy('/ve026a/set_motor', SetBool)(True)
print(res.message)
except rospy.ServiceException, e:
print("Service call failed, %s" % e)
msg = JointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
p1 = JointTrajectoryPoint()
p1.positions = [0, 0, 0, 0, 0, 0]
p1.velocities = [0, 0, 0, 0, 0, 0]
p1.time_from_start = rospy.Duration.from_sec(3.0)
p2 = JointTrajectoryPoint()
p2.positions = [0.5, 0.5, 0, 0, 0, 0]
p2.velocities = [0, 0, 0, 0, 0, 0]
p2.time_from_start = rospy.Duration.from_sec(5.0)
p3 = JointTrajectoryPoint()
p3.positions = [0, 0, 0, 0, 0, 0]
p3.velocities = [0, 0, 0, 0, 0, 0]
p3.time_from_start = rospy.Duration.from_sec(7.0)
msg.points = [p1, p2, p3]
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:
start_time = time()
### Forward Kinematics code
# Create symbols
#Joint Angle Symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
#Link Offset Symbols
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
#Link Length symbols
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
#Twist Angle symbols
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
# Modified DH parameters
DH_Table = {
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 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(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_EE = TF_Matrix(alpha6, a6, d7, q7).subs(DH_Table)
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
# Extract rotation matrices from the transformation matrices
R0_3_Temp = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
# Define Roatation Matrix from End Effector Yaw, Roll, Pitch
r, p, y = symbols('r p y')
ROT_x = Matrix([
[1, 0, 0], #Roll
[0, cos(r), -sin(r)],
[0, sin(r), cos(r)]
])
ROT_y = Matrix([
[cos(p), 0, sin(p)], #Pitch
[0, 1, 0],
[-sin(p), 0, cos(p)]
])
ROT_z = Matrix([
[cos(y), -sin(y), 0], #Yaw
[sin(y), cos(y), 0],
[0, 0, 1]
])
ROT_EE = ROT_z * ROT_y * ROT_x
## Initializtion
# Define Joint angles for Target Spawn locations for the END trajectory point
#(theta1i,theta3i,theta4i,theta5i,theta6i, theta7i, theta8i, theta9i) are the joint angles for target sawn locations 1 to 9
# where i ranges from 1 to 6, while moving from initial location to target spawn location )
theta1 = 0
theta2 = 0
theta3 = 0
theta4 = 0
theta5 = 0
theta6 = 0
theta61 = -0.460788639682829
theta62 = -1.4083946795092 + pi / 2
theta63 = -1.49141431818708 + pi / 2
theta64 = -1.0235104903567 + pi
theta65 = 0.495695716305110
theta66 = -pi + 0.972495930022601
theta41 = 0.466568380940290
theta42 = -1.40588840950334 + pi / 2
theta43 = -1.49933810543832 + pi / 2
theta44 = -pi + 1.12482883075263
theta45 = 0.521928836050086
theta46 = -1.06984849582538 + pi
theta91 = -0.460620971603582
theta92 = -1.41600678927374 + pi / 2
theta93 = -2.01520122842406 + pi / 2
theta94 = 0.912819391695163
theta95 = 0.538148746753259
theta96 = -0.855105912394959
theta71 = 0.460651236883725
theta72 = -1.41603690529533 + pi / 2
theta73 = -2.015158587665 + pi / 2
theta74 = -0.913449451374226
theta75 = 0.537900753574318
theta76 = 0.855694311433023
theta81 = -4.02985219558988e-6
theta82 = -1.59017523803714 + pi / 2
theta83 = -1.81801235853875 + pi / 2
theta84 = -8.71196937061172e-5
theta85 = 0.259284638790722
theta86 = -7.35675934196834e-6
theta11 = 0.461944532159549
theta12 = -1.11836645228303 + pi / 2
theta13 = -1.31463177224073 + pi / 2
theta14 = -pi + 0.725225037468592
theta15 = 0.676095580016169
theta16 = -0.62726276643435 + pi
theta51 = -0.00229054389158514
theta52 = -1.57765784191287 + pi / 2
theta53 = -1.32851692115255 + pi / 2
theta54 = -0.086229482068646 + pi
theta55 = 0.181470117894438
theta56 = -pi + 0.488456526793796
theta31 = -0.466518984306794
theta32 = -1.08486429666301 + pi / 2
theta33 = -1.31281014892211 + pi / 2
theta34 = -0.618700551351137 + pi
theta35 = 0.886705067213612
theta36 = -pi + 0.441222764426423
# Define Joint angle for the end trajectory point
# (thetaei) is the joint angle while moving from target spawn location to bin)
thetae1 = -1.3683474710526 + pi
thetae2 = -0.965048732979534 + pi / 2
thetae3 = -2.09403813236587 + pi / 2
thetae4 = -1.55313297331450
thetae5 = -1.3697836855635 + pi
thetae6 = -1.48647561055299 + pi
# Initialize service response
ReachCount = 0
joint_trajectory_list = []
ee_offset_list = [] # Create List of error in end effector
time_list = [] # Create list of time samples
if (len(req.poses) % 2) == 0: # For loop initiation of req.poses
i = 1
else:
i = 0
if (len(req.poses) > 50): # For loop initiation of req.poses
j = 2
else:
j = 1
i = 0
Count.count = Count.count + 1 # Incrementing Iteration count for number of Inverse Kinematics loop
print("IK count: %s" % (Count.count))
# Calcukating Target Spawn END locations on shelf from request responses
goal_px = req.poses[len(req.poses) - 1].position.x
goal_py = req.poses[len(req.poses) - 1].position.y
goal_pz = req.poses[len(req.poses) - 1].position.z
print("\nGoal position along x axis: %04.8f" % goal_px)
print("Goal position along y axis: %04.8f" % goal_py)
print("Goal position along z axis: %04.8f" % goal_pz)
Target_Spawn = 0
if ((Count.count % 2) == 1):
if (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -1) & (
goal_py < -0.8) & (goal_pz >= 1.58) & (
goal_pz <= 1.59): #Target Spawn location: 6 on shelf
Target_Spawn = 6
elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= 0.8) & (
goal_py < 1) & (goal_pz >= 2.3) & (
goal_pz <= 2.5): #Target Spawn location: 7 on shelf
Target_Spawn = 7
elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= 0.8) & (
goal_py < 1) & (goal_pz >= 1.58) & (
goal_pz <= 1.59): #Target Spawn location: 4 on shelf
Target_Spawn = 4
elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -1) & (
goal_py < -0.8) & (goal_pz >= 2.3) & (
goal_pz <= 2.5): #Target Spawn location: 9 on shelf
Target_Spawn = 9
elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -0.1) & (
goal_py < 0.1) & (goal_pz >= 2.3) & (
goal_pz <= 2.5): #Target Spawn location: 8 on shelf
Target_Spawn = 8
elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= 0.8) & (
goal_py < 1) & (goal_pz >= 0.7) & (
goal_pz <= 0.9): #Target Spawn location: 1 on shelf
Target_Spawn = 1
elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -1) & (
goal_py < -0.8) & (goal_pz >= 0.7) & (
goal_pz <= 0.9): #Target Spawn location: 3 on shelf
Target_Spawn = 3
elif (goal_px >= 2.08) & (goal_px <= 2.1) & (goal_py >= -0.1) & (
goal_py < 0.1) & (goal_pz >= 1.58) & (
goal_pz <= 1.59): #Target Spawn location: 5 on shelf
Target_Spawn = 5
print("\nTarget spawn location is: %s" % (Target_Spawn))
for x in xrange(i, len(req.poses), j):
joint_trajectory_point = JointTrajectoryPoint()
print(x)
# 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
Rot_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs(
p, radians(-90))
ROT_EE = ROT_EE * Rot_Error
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
# Calculate End Effetor position
EE = Matrix([[px], [py], [pz]])
# Wrist Center Calculation
WC = EE - (0.303) * ROT_EE[:, 2]
### Inverse Kinematics code
if len(joint_trajectory_list
) > 2: #Calculate previous joint angles
z = (joint_trajectory_list[len(joint_trajectory_list) - 1])
theta1 = z.positions[0]
theta2 = z.positions[1]
theta3 = z.positions[2]
theta4 = z.positions[3]
theta5 = z.positions[4]
theta6 = z.positions[5]
if (len(joint_trajectory_list) > 2) & (
(Count.count % 2) == 0) & (x < (len(req.poses) - 1)) & (
theta1 > -1.62 + pi) & (theta1 < -1.36 + pi) & (
theta2 > -1.1 + pi / 2) & (theta2 < -0.94 + pi / 2) & (
theta3 > -2.2 + pi / 2) & (theta3 < -1.9 + pi / 2):
print('Reached TargetLocation'
) # Kuka arm has reached near the bin
if (ReachCount == 0):
ReachCount = 1
joint_trajectory_point.positions = [
thetae1, thetae2, thetae3, thetae4, thetae5, thetae6
]
joint_trajectory_list.append(joint_trajectory_point)
else:
# Calculate joint angles using Geometric IK method
theta1 = atan2(WC[1], WC[0])
side_b = sqrt(
pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow(
(WC[2] - 0.75), 2)) #side_a = 1.501 #side_c = 1.25
if ((1.25 + 1.501) > side_b) & ((1.25 + side_b) > 1.501) & (
(side_b + 1.501) > 1.25
): # Mathematically, (sum of 2 sides of a triangle) > 3rd side
angle_a = acos(
(side_b * side_b + 1.25 * 1.25 - 1.501 * 1.501) /
(2 * side_b * 1.25))
angle_b = acos(
(1.501 * 1.501 + 1.25 * 1.25 - side_b * side_b) /
(2 * 1.501 * 1.25))
angle_c = acos(
(1.501 * 1.501 + side_b * side_b - 1.25 * 1.25) /
(2 * 1.501 * 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 + atan2(0.054, 1.5))
R0_3 = R0_3_Temp.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3
}) # Evaluating Rotation Matrix
R3_6 = R0_3.inv("LU") * ROT_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])
# Forward Kinematics evaluation with derived joint angles
FK = T0_EE.evalf(
subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
# Error Calculation of End Effector position by comparing with end-effector poses, received as input
ee_x_e = abs(FK[0, 3] -
px) # End Effector error offset along x axis
ee_y_e = abs(FK[1, 3] -
py) # End Effector error offset along y axis
ee_z_e = abs(FK[2, 3] -
pz) # End Effector error offset along z axis
# Overall end-effector offset
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)
# Populate response for the IK request
if (x == len(req.poses) - 1) & (
Target_Spawn == 6
): #Last joint angle calculation for Target Spawn location: 6
if ((Count.count % 2) == 1):
joint_trajectory_point.positions = [
theta61, theta62, theta63, theta64, theta65,
theta66
]
joint_trajectory_list.append(
joint_trajectory_point)
elif (x == len(req.poses) - 1) & (
Target_Spawn == 4
): #Last joint angle calculation for Target Spawn location: 4
if ((Count.count % 2) == 1):
joint_trajectory_point.positions = [
theta41, theta42, theta43, theta44, theta45,
theta46
]
joint_trajectory_list.append(
joint_trajectory_point)
elif (x == len(req.poses) - 1) & (
Target_Spawn == 9
): #Last joint angle calculation for Target Spawn location: 9
if ((Count.count % 2) == 1):
joint_trajectory_point.positions = [
theta91, theta92, theta93, theta94, theta95,
theta96
]
joint_trajectory_list.append(
joint_trajectory_point)
elif (x == len(req.poses) - 1) & (
Target_Spawn == 7
): #Last joint angle calculation for Target Spawn location: 7
if ((Count.count % 2) == 1):
joint_trajectory_point.positions = [
theta71, theta72, theta73, theta74, theta75,
theta76
]
joint_trajectory_list.append(
joint_trajectory_point)
elif (x == len(req.poses) - 1) & (
Target_Spawn == 8
): #Last joint angle calculation for Target Spawn location: 8
if ((Count.count % 2) == 1):
joint_trajectory_point.positions = [
theta81, theta82, theta83, theta84, theta85,
theta86
]
joint_trajectory_list.append(
joint_trajectory_point)
elif (x == len(req.poses) - 1) & (
Target_Spawn == 1
): #Last joint angle calculation for Target Spawn location: 1
if ((Count.count % 2) == 1):
joint_trajectory_point.positions = [
theta11, theta12, theta13, theta14, theta15,
theta16
]
joint_trajectory_list.append(
joint_trajectory_point)
elif (x == len(req.poses) - 1) & (
Target_Spawn == 5
): #Last joint angle calculation for Target Spawn location: 5
if ((Count.count % 2) == 1):
joint_trajectory_point.positions = [
theta51, theta52, theta53, theta54, theta55,
theta56
]
joint_trajectory_list.append(
joint_trajectory_point)
elif (x == len(req.poses) - 1) & (
Target_Spawn == 3
): #Last joint angle calculation for Target Spawn location: 3
if ((Count.count % 2) == 1):
joint_trajectory_point.positions = [
theta31, theta32, theta33, theta34, theta35,
theta36
]
joint_trajectory_list.append(
joint_trajectory_point)
elif (x == len(req.poses) - 1) & (
(Count.count % 2)
== 0): #Last joint angle calculation towards Bin
if (ReachCount == 0):
if (ee_offset < 0.01):
joint_trajectory_point.positions = [
theta1, theta2, theta3, thetae4, thetae5,
thetae6
]
joint_trajectory_list.append(
joint_trajectory_point)
else:
joint_trajectory_point.positions = [
thetae1, thetae2, thetae3, thetae4,
thetae5, thetae6
]
joint_trajectory_list.append(
joint_trajectory_point)
zt = (joint_trajectory_list[
len(joint_trajectory_list) - 2])
Theta_6 = zt.positions[5]
Theta6Diff = (np.absolute(Theta_6 - thetae6))
if (Theta6Diff >= 1.7):
joint_trajectory_point.positions = [
thetae1, thetae2, thetae3, thetae4,
thetae5, Theta_6
]
joint_trajectory_list.append(
joint_trajectory_point)
elif (
ee_offset < 0.01
): #Allow joint angles for which error is below threshold
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
ee_offset_list.append(ee_offset)
t = time()
time_list.append(t)
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
## Plotting error outputs of end effector w.r.t to received responses (Close the plot window for code to run further)
## UNCOMMENT BELOW LINES TO CHECK THE ERROR PLOTS FOR CHALLENGE COMPLETION
#ee_offset_ar = np.array([ee_offset_list])
#time_list_ar = np.array([time_list])
#plt.figure(figsize=(15,7))
#plt.rc('font', family='serif', size=15)
#plt.plot(time_list_ar,ee_offset_ar, marker='o', color='r')
#plt.xlabel('Time',fontsize=18)
#plt.ylabel('End Effector Offset',fontsize=18)
#plt.grid(True)
#plt.show()
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_,
y=recog_pos.pose.position.y - 0.012,
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
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})
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)
# goal_pose =PoseStamped()
# goal_pose.pose.position.x=recog_pos.pose.position.x+0.4
# goal_pose.pose.position.y=recog_pos.pose.position.y+0.2
# goal_pose.pose.position.z=recog_pos.pose.position.z
# goal_pose.pose.orientation.x=recog_pos.pose.orientation.x
# goal_pose.pose.orientation.y=recog_pos.pose.orientation.y
# goal_pose.pose.orientation.z=recog_pos.pose.orientation.z
# goal_pose.pose.orientation.w=recog_pos.pose.orientation.w
# yaw=math.pi/6
# quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
# #type(pose) = geometry_msgs.msg.Pose
# nav_goal = move_base_msgs.msg.MoveBaseActionGoal()
# nav_goal.header.stamp=rospy.Time(0)
# nav_goal.goal.target_pose.header.frame_id = "map"
# nav_goal.goal.target_pose.pose.position.x=recog_pos.pose.position.x+0.3
# nav_goal.goal.target_pose.pose.position.y=recog_pos.pose.position.y+0.2
# nav_goal.goal.target_pose.pose.position.z=0.0
# nav_goal.goal.target_pose.pose.orientation.x=quaternion[0]
# nav_goal.goal.target_pose.pose.orientation.y=quaternion[1]
# nav_goal.goal.target_pose.pose.orientation.z=quaternion[2]
# nav_goal.goal.target_pose.pose.orientation.w=quaternion[3]
# base_pub.publish(nav_goal)
# # 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.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
# # whole_body.move_end_effector_by_line((0.7071, 0.7071,0), 0.5)
# # whole_body.move_end_effector_by_line((0, -0.7071, 0.7071), 0.5)
# whole_body.impedance_config= None
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
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)
gripper.command(1.0)
# whole_body.move_end_effector_by_line((0, 0, -1), 0.25)
tw_cmd = geometry_msgs.msg.Twist()
tw_cmd.linear.x = -1.2
vel_pub.publish(tw_cmd)
rospy.sleep(2.0)
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()
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:
### Your FK code here
# Create symbols
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: q2 - pi / 2,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2,
a3: -0.054,
d4: 1.50,
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 DHTmatrix(q, d, a, alpha):
DHTmatrix = 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 DHTmatrix
# Create individual transformation matrices
T0_1 = DHTmatrix(q1, d1, a0, alpha0).subs(s)
T1_2 = DHTmatrix(q2, d2, a1, alpha1).subs(s)
T2_3 = DHTmatrix(q3, d3, a2, alpha2).subs(s)
T3_4 = DHTmatrix(q4, d4, a3, alpha3).subs(s)
T4_5 = DHTmatrix(q5, d5, a4, alpha4).subs(s)
T5_6 = DHTmatrix(q6, d6, a5, alpha5).subs(s)
T6_7 = DHTmatrix(q7, d7, a6, alpha6).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
])
r, p, y = symbols('r p y')
# Define Roll Matrix about x-axis
R_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0,
sin(r),
cos(r)]])
# Define Pitch Matrix about y-axis
R_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0,
cos(p)]])
# Define Yaw Matrix about z-axis
R_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0,
1]])
# Compensate for rotation discrepancy between DH parameters and Gazebo
R_corr = R_z.subs(y, pi) * R_y.subs(p, -pi / 2)
Rrpy = R_x.subs(r, roll) * R_y.subs(p, pitch) * R_z.subs(
y, yaw) * R_corr
# Calculate Wrist Center position
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303) * Rrpy[:, 2]
# Calculate joint angles using Geometric IK method
theta1 = atan2(WC[1], WC[0])
# Calculate Sides of Triangle Formed by Origin 2, Origin 3, and the WC Origin
side_a = 1.5009716852759081944053710341178
side_b = sqrt((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)**2 +
(WC[2] - 0.75)**2)
side_c = 1.25
# Calculate Angles of Triangle Formed by Origin 2, Origin 3, and the WC Origin
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))
# Calculate theta2 and theta3
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)
# Calculate theta4, theta5, and theta6
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") * Rrpy
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 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
print("Total poses: " + str(len(req.poses)))
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
])
pwc_ee = Matrix([px, py, pz])
### Your IK code here
# R_rpy_eval = R_rpy.evalf(subs={r: roll, p: pitch, y: yaw})
R_rpy_eval = R_rpy.subs({r: roll, p: pitch, y: yaw})
# Get the wrist-center
p0_wc = pwc_ee - (
s[d7]) * R_rpy_eval[:, 2] # wc in terms of the base link
# Calculate joint angles using Geometric IK method
#
#
# Using law of cosines to get theta 1, 2 and 3
# set the length of the sides of the triangle joint2, 3 and 4
side_a = s[d4]
side_b = sqrt(
pow((sqrt(p0_wc[0]**2 + p0_wc[1]**2) - s[a1]), 2) + pow(
(p0_wc[2] - s[d1]), 2)) # i dont know why?!
side_c = s[a2]
# Get the angles
angle_a = abs(
acos((side_b**2 + side_c**2 - side_a**2) /
(2 * side_b * side_c)))
angle_b = abs(
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))
# Get theta1, 2 and 3
# using pythagorean. Draw the kinematics out to understand
theta1 = atan2(p0_wc[1], p0_wc[0])
# theta2 is a subset of angle_a, so we remove the angle that's not related to theta2
theta2 = pi / 2 - angle_a - atan2(
p0_wc[2] - s[d1],
sqrt(p0_wc[0]**2 + p0_wc[1]**2) - s[a1])
# theta3 is a subset of angle_b, so we remove the angle that is not theta3
sag_angle = atan2(0.054, sqrt(1.501**2 - 0.054**2))
theta3 = pi / 2 - (angle_b + sag_angle)
# Get the rotation from joint 3 to the ee
# evaluate the rotation from base link to link3 (joint 0 to joint 3)
# because we now have the theta for these joints that defines the pose of the wrist center
R0_3_eval = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
# The inverse of R0_3_eval yields R3_0, R_rpy_eval is
# R3_6 = R0_3_eval.inv("LU") * R_rpy_eval
R3_6 = R0_3_eval.transpose() * R_rpy_eval
# We get theta4, 5, 6 from R3_G which now contains the evaluated
# rotation from base link to wrist center
theta5 = atan2(sqrt(R3_6[0, 2]**2 + R3_6[2, 2]**2), R3_6[1, 2])
# If sin(theta5) is negative, switch appropriate sign to keep values positive
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])
# Generate and print the error from the computed and expected end
# effector
# get the computed end effector using the computed thetas
your_ee = T0_G.evalf(
subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})[:3, 3]
ee_x_e = abs(your_ee[0] - pwc_ee[0])
ee_y_e = abs(your_ee[1] - pwc_ee[1])
ee_z_e = abs(your_ee[2] - pwc_ee[2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("Computed EE: ")
pprint(pwc_ee)
print("Expected EE:")
pprint(your_ee)
print("End 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)
# 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 interpolated_ik_motion_planner_callback(self, req):
#names and angles for the joints in their desired order
joint_names = req.motion_plan_request.start_state.joint_state.name
start_angles = req.motion_plan_request.start_state.joint_state.position
#sanity-checking: joint_names and start_angles should be the same length, if any start_angles are specified
if start_angles and len(joint_names) != len(start_angles):
rospy.logerr("start_state.joint_state.name needs to be the same length as start_state.joint_state.position! Quitting")
return 0
#reorder the start angles to the order needed by IK
reordered_start_angles = []
#get the current joint states for the robot
#joint_states_msg = rospy.wait_for_message('joint_states', JointState, 10.0)
#if not joint_states_msg:
# rospy.logerr("unable to get joint_states message")
# return 0
#get the desired start angles for each IK arm joint in turn from start_state.joint_state.position
#(use the current angle if not specified)
for joint_name in self.ik_utils.joint_names:
#desired start angle specified
if joint_name in joint_names and start_angles:
index = joint_names.index(joint_name)
reordered_start_angles.append(start_angles[index])
else:
rospy.logerr("missing joint angle, can't deal")
return 0
#desired start angle not specified, use the current angle
#elif 0: #joint_name in joint_states_msg.name:
# index = joint_states_msg.name.index(joint_name)
# current_position = joint_states_msg.position[index]
# reordered_start_angles.append(current_position)
#malformed joint_states message?
# else:
# rospy.logerr("an expected arm joint,"+joint_name+"was not found!")
# return 0
#get additional desired joint angles (such as for the gripper) to pass through to IK
additional_joint_angles = []
additional_joint_names = []
for (ind, joint_name) in enumerate(joint_names):
if joint_name not in self.ik_utils.joint_names:
#rospy.loginfo("found %s"%joint_name)
additional_joint_angles.append(start_angles[ind])
additional_joint_names.append(joint_name)
IK_robot_state = None
if additional_joint_angles:
#rospy.loginfo("adding additional start angles for:"+str(additional_joint_names))
#rospy.loginfo("additional joint angles:"+str(additional_joint_angles))
IK_robot_state = RobotState()
IK_robot_state.joint_state.name = additional_joint_names
IK_robot_state.joint_state.position = additional_joint_angles
#check that the desired link is in the list of possible IK links (only r/l_wrist_roll_link for now)
link_name = req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids[0]
if link_name != self.ik_utils.link_name:
rospy.logerr("link_name not allowed: %s"%link_name)
return 0
#the start pose for that link
start_pose = req.motion_plan_request.start_state.multi_dof_joint_state.poses[0]
#the frame that start pose is in
frame_id = req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids[0]
#turn it into a PoseStamped
start_pose_stamped = self.add_header(PoseStamped(), frame_id)
start_pose_stamped.pose = start_pose
#the desired goal position
goal_pos = req.motion_plan_request.goal_constraints.position_constraints[0].position
#the frame that goal position is in
goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id
#convert the position to base_link frame
goal_ps = self.add_header(PointStamped(), goal_pos_frame)
goal_ps.point = goal_pos
goal_pos_list = self.ik_utils.point_stamped_to_list(goal_ps, 'base_link')
#the desired goal orientation
goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation
#the frame that goal orientation is in
goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id
#convert the quaternion to base_link frame
goal_qs = self.add_header(QuaternionStamped(), goal_quat_frame)
goal_qs.quaternion = goal_quat
goal_quat_list = self.ik_utils.quaternion_stamped_to_list(goal_qs, 'base_link')
#assemble the goal pose into a PoseStamped
goal_pose_stamped = self.add_header(PoseStamped(), 'base_link')
goal_pose_stamped.pose = Pose(Point(*goal_pos_list), Quaternion(*goal_quat_list))
#get the ordered collision operations, if there are any
ordered_collision_operations = None #req.motion_plan_request.ordered_collision_operations
#if ordered_collision_operations.collision_operations == []:
# ordered_collision_operations = None
#get the link paddings, if there are any
link_padding = None #req.motion_plan_request.link_padding
#if link_padding == []:
# link_padding = None
#RUN! Check the Cartesian path for consistent, non-colliding IK solutions
(trajectory, error_codes) = self.ik_utils.check_cartesian_path(start_pose_stamped, \
goal_pose_stamped, reordered_start_angles, self.pos_spacing, self.rot_spacing, \
self.consistent_angle, self.collision_aware, self.collision_check_resolution, \
self.steps_before_abort, self.num_steps, ordered_collision_operations, \
self.start_from_end, IK_robot_state, link_padding)
#find appropriate velocities and times for the valid part of the resulting joint path (invalid parts set to 0)
#if we're searching from the end, keep the end; if we're searching from the start, keep the start
start_ind = 0
stop_ind = len(error_codes)
if self.start_from_end:
for ind in range(len(error_codes)-1, 0, -1):
if error_codes[ind]:
start_ind = ind+1
break
else:
for ind in range(len(error_codes)):
if error_codes[ind]:
stop_ind = ind
break
(times, vels) = self.ik_utils.trajectory_times_and_vels(trajectory[start_ind:stop_ind], self.max_joint_vels, self.max_joint_accs)
times = [0]*start_ind + times + [0]*(len(error_codes)-stop_ind)
vels = [[0]*7]*start_ind + vels + [[0]*7]*(len(error_codes)-stop_ind)
rospy.logdebug("trajectory:")
for ind in range(len(trajectory)):
rospy.logdebug("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
rospy.logdebug("")
for ind in range(len(trajectory)):
rospy.logdebug("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind]))
#the response
res = GetMotionPlanResponse()
#the arm joint names in the normal order, as spit out by IKQuery
res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:]
#a list of 7-lists of joint angles, velocities, and times for a trajectory that gets you from start to goal
#(all 0s if there was no IK solution for a point on the path)
res.trajectory.joint_trajectory.points = []
for i in range(len(trajectory)):
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = trajectory[i]
joint_trajectory_point.velocities = vels[i]
joint_trajectory_point.time_from_start = rospy.Duration(times[i])
res.trajectory.joint_trajectory.points.append(joint_trajectory_point)
#a list of ArmNavigationErrorCodes messages, one for each trajectory point, with values as follows:
#ArmNavigationErrorCodes.SUCCESS (1): no problem
#ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided)
#ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-20): inconsistency in path between this point and the next point
#ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution)
#ArmNavigationErrorCodes.PLANNING_FAILED (0): aborted before getting to this point
error_code_dict = {0:ArmNavigationErrorCodes.SUCCESS, 1:ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED, \
2:ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED, 3:ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED, \
4:ArmNavigationErrorCodes.PLANNING_FAILED}
trajectory_error_codes = [ArmNavigationErrorCodes(val=error_code_dict[error_code]) for error_code in error_codes]
res.trajectory_error_codes = trajectory_error_codes
res.error_code.val = ArmNavigationErrorCodes.SUCCESS
# rospy.loginfo("trajectory:")
# for ind in range(len(trajectory)):
# rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
# rospy.loginfo("")
# for ind in range(len(trajectory)):
# rospy.loginfo("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind]))
return res
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
#
#
###
d6_val = 0.0
d1_val = 0.75
a1_val = 0.35
a2_val = 1.25
a3_val = -0.054
d4_val = 1.50
l = 0.303
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')
s = {
alpha0: 0.0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: -pi / 2,
a1: 0.35,
d2: 0,
q2: q2 - np.pi / 2,
alpha2: 0.0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2,
a3: -0.054,
d4: 1.50,
q4: q4,
alpha4: pi / 2,
a4: 0,
d5: 0,
q5: q5,
alpha5: -pi / 2,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0.0,
a6: 0,
d7: 0.303,
q7: 0
}
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(alpha6)*d7],
[ sin(q7)*sin(alpha6), cos(q7)*sin(alpha6), cos(alpha6), cos(alpha6)*d7],
[ 0, 0,0, 1]])
#T6_G = T6_G.subs(s)
"""
T0_2 = T0_1 * T1_2
T0_3 = 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_G = simplify(T0_6 * T6_G)
R_z = Matrix([[cos(np.pi), -sin(np.pi), 0],
[sin(np.pi), cos(np.pi), 0], [0, 0, 1]])
R_y = Matrix([[cos(-np.pi / 2), 0, sin(-np.pi / 2)], [0, 1, 0],
[-sin(-np.pi / 2), 0,
cos(-np.pi / 2)]])
R_corr = simplify(R_z * R_y)
#T_total = simplify(T0_G * R_corr)
# 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
])
p1, p2, p3 = symbols('p1:4')
Ry = rot_z(p1).subs({p1: yaw})
Rp = rot_y(p2).subs({p2: pitch})
Rr = rot_x(p3).subs({p3: roll})
Rrpy = simplify(Ry * Rp * Rr * R_corr)
wx = px - (d6_val + l) * Rrpy[0, 2]
wy = py - (d6_val + l) * Rrpy[1, 2]
wz = pz - (d6_val + l) * Rrpy[2, 2]
#print wx, wy, wz
theta1 = atan2(wy, wx)
#print theta1
A = sqrt(a3_val**2 + d4_val**2)
B = sqrt((wz - d1_val)**2 + (sqrt(wy**2 + wx**2) - a1_val)**2)
C = a2_val
a = acos((-A**2 + B**2 + C**2) / (2 * C * B))
b = acos((A**2 - B**2 + C**2) / (2 * A * C))
c = acos((A**2 + B**2 - C**2) / (2 * A * B))
#print a, b, c
theta2 = np.pi / 2 - a - atan2((wz - d1_val),
(sqrt(wy**2 + wx**2) - a1_val))
theta3 = np.pi / 2 - (b - atan2(a3_val, d4_val))
#print theta1, theta2, theta3
R0_3 = T0_3[:3, :3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * Rrpy
#R = (T3_4 * T4_5 * T5_6)[0:3, 0:3]
#R = simplify(R)
#print R
"""
for i in range(9):
print R[i]
print i
if i+1 % 3 == 0:
print "\n\n\n"
"""
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
fail = false
theta5 = atan2(sqrt(R3_6[1, 0]**2 + R3_6[1, 1]**2), R3_6[1, 2])
theta5_old = 0
if np.abs(sin(theta5) * cos(theta6) - R3_6[1, 0]) > 1e-3 or np.abs(
sin(theta5) * sin(theta6) + R3_6[1, 1]) > 1e-3:
theta5_old = theta5
theta5 = atan2(-sqrt(R3_6[1, 0]**2 + R3_6[1, 1]**2), R3_6[1,
2])
if np.abs(sin(theta5) * cos(theta6) -
R3_6[1, 0]) > 1e-3 or np.abs(
sin(theta5) * sin(theta6) + R3_6[1, 1]) > 1e-3:
rospy.logwarn("Solution not found for theta5")
fail = true
#print theta4, theta5, theta6
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
#
#
# Calculate joint angles using Geometric IK method
#
#
###
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
if fail:
ee_calculated1 = forward_kinematics(
[theta1, theta2, theta3, theta4, theta5, theta6])
ee_calculated2 = forward_kinematics(
[theta1, theta2, theta3, theta4, theta5_old, theta6])
# print ee_calculated
error1 = []
error1.append(np.abs(ee_calculated1[0] - px))
error1.append(np.abs(ee_calculated1[1] - py))
error1.append(np.abs(ee_calculated1[2] - pz))
overall_err1 = sqrt(error1[0]**2 + error1[1]**2 + error1[2]**2)
error2 = []
error2.append(np.abs(ee_calculated2[0] - px))
error2.append(np.abs(ee_calculated2[1] - py))
error2.append(np.abs(ee_calculated2[2] - pz))
overall_err2 = sqrt(error2[0]**2 + error2[1]**2 + error2[2]**2)
if overall_err1 < overall_err2:
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
rospy.loginfo(
str(sum(ee_calculated1)) + "\tError " + str(x) + "\t" +
": " + "\t" + str(error1) + "\tOverall error: " +
str(overall_err1) + " other: " + str(overall_err2))
else:
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5_old, theta6
]
rospy.loginfo(
str(sum(ee_calculated2)) + "\tError " + str(x) + "\t" +
": " + "\t" + str(error2) + "\tOverall error: " +
str(overall_err2) + " other: " + str(overall_err1))
else:
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
ee_calculated = forward_kinematics(
joint_trajectory_point.positions)
#print ee_calculated
error = []
error.append(np.abs(ee_calculated[0] - px))
error.append(np.abs(ee_calculated[1] - py))
error.append(np.abs(ee_calculated[2] - pz))
overall_err = sqrt(error[0]**2 + error[1]**2 + error[2]**2)
rospy.loginfo(
str(sum(ee_calculated)) + "\tError " + str(x) + "\t" +
": " + "\t" + str(error) + "\tOverall error: " +
str(overall_err))
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
print 'moving the right arm out of the way'
whicharm = 'r'
arm_joint_trajectory_action_name = '' + whicharm + '_arm_controller/joint_trajectory_action'
arm_joint_action_client = actionlib.SimpleActionClient(
arm_joint_trajectory_action_name, JointTrajectoryAction)
arm_joint_action_client.wait_for_server()
arm_goal = JointTrajectoryGoal()
arm_goal.trajectory.joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint',
'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
arm_pt = JointTrajectoryPoint()
arm_pt.positions = [-pi / 2, 0, 0, 0, 0, 0, 0]
arm_pt.time_from_start = rospy.Duration(5.0)
arm_goal.trajectory.points.append(arm_pt)
arm_joint_action_client.send_goal_and_wait(arm_goal)
print 'arm action state:', actionlib.get_name_of_constant(
actionlib.GoalStatus, arm_joint_action_client.get_state())
print 'moving the left arm to problematic configuration'
whicharm = 'l'
arm_joint_trajectory_action_name = '' + whicharm + '_arm_controller/joint_trajectory_action'
arm_joint_action_client = actionlib.SimpleActionClient(
arm_joint_trajectory_action_name, JointTrajectoryAction)
print 'waiting for', arm_joint_trajectory_action_name
print 'have you set ROS_IP?'
def __init__(self):
porcentaje = 0.7 #0.5
porcentaje1 = 0.7 #1
porcentaje16 = 0.8 # 1
des6 = 0.1 # 0.15
giro = 0
x = loadmat('estruc_piernas_fase3.mat')
# print x
piernas_der = x['estruc_piernaDer_fase']
art1_der = piernas_der[0, 0]
art2_der = piernas_der[0, 1]
art3_der = piernas_der[0, 2]
art4_der = piernas_der[0, 3]
art5_der = piernas_der[0, 4]
art6_der = piernas_der[0, 5]
piernas_izq = x['estruc_piernaIzq_fase']
art1_izq = piernas_izq[0, 0]
art2_izq = piernas_izq[0, 1]
art3_izq = piernas_izq[0, 2]
art4_izq = piernas_izq[0, 3]
art5_izq = piernas_izq[0, 4]
art6_izq = piernas_izq[0, 5]
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
piernas_joints = [
'cilinder_blue_box1_der_joint', 'cilinder_blue1_box2_der_joint',
'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint',
'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
'cilinder_blue_box1_izq_joint', 'cilinder_blue1_box2_izq_joint',
'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint',
'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint'
]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient(
'/piernas/piernas_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
#/piernas/piernas_controller/follow_joint_trajectory
arm_client.wait_for_server()
rospy.loginfo('...connected.')
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = piernas_joints
# piernas_goalF0 = [0, -0.3, -0.4, 0.8, -0.3, -0.4,
# 0, -0.4, -0.3, 0.6, +0.4, -0.3]
# arm_trajectory.points.append(JointTrajectoryPoint())
# arm_trajectory.points[0].positions = piernas_goalF0
# arm_trajectory.points[0].velocities = [0.0 for k in piernas_goalF0]
# arm_trajectory.points[0].accelerations = [0.0 for k in piernas_goalF0]
# arm_trajectory.points[0].time_from_start = rospy.Duration(2)
piernas_goalF01 = [
giro * porcentaje * art1_der['senialCompleta'][0, 50],
0.8 * porcentaje * art2_der['senialCompleta'][0, 50],
porcentaje1 * art3_der['senialCompleta'][0, 50],
porcentaje1 * art4_der['senialCompleta'][0, 50],
0.8 * porcentaje * art5_der['senialCompleta'][0, 50],
porcentaje16 * art6_der['senialCompleta'][0, 50] + des6,
giro * porcentaje * art1_izq['senialCompleta'][0, 50],
0.8 * porcentaje * art2_izq['senialCompleta'][0, 50],
porcentaje1 * art3_izq['senialCompleta'][0, 50],
porcentaje1 * art4_izq['senialCompleta'][0, 50],
0.8 * porcentaje * art5_izq['senialCompleta'][0, 50],
porcentaje16 * art6_izq['senialCompleta'][0, 50] + des6
]
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = piernas_goalF01
arm_trajectory.points[0].velocities = [0.0 for k in piernas_goalF01]
arm_trajectory.points[0].accelerations = [0.0 for k in piernas_goalF01]
arm_trajectory.points[0].time_from_start = rospy.Duration(2)
ind = 1
salto = 4
for i in range(50, int(art1_der['senialCompleta'].shape[1] * 0.25),
salto):
# print ind
# print int(art1_der['senialCompleta'].shape[1]*0.2)
piernas_goalF = [
giro * porcentaje * art1_der['senialCompleta'][0, i],
0.8 * porcentaje * art2_der['senialCompleta'][0, i],
porcentaje1 * art3_der['senialCompleta'][0, i],
porcentaje1 * art4_der['senialCompleta'][0, i],
0.8 * porcentaje * art5_der['senialCompleta'][0, i],
porcentaje16 * art6_der['senialCompleta'][0, i] + des6,
giro * porcentaje * art1_izq['senialCompleta'][0, i],
0.8 * porcentaje * art2_izq['senialCompleta'][0, i],
porcentaje1 * art3_izq['senialCompleta'][0, i],
porcentaje1 * art4_izq['senialCompleta'][0, i],
0.8 * porcentaje * art5_izq['senialCompleta'][0, i],
porcentaje16 * art6_izq['senialCompleta'][0, i] + des6
]
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[ind].positions = piernas_goalF
arm_trajectory.points[ind].velocities = [
0.0 for j in piernas_goalF
]
arm_trajectory.points[ind].accelerations = [
0.0 for j in piernas_goalF
]
# art1_der['tiempo_completo'][0, i]
# 0.01 * (ind + 1)
arm_trajectory.points[ind].time_from_start = rospy.Duration(
2 + (porcentaje1 * art1_der['tiempo_completo'][0, i]) * 1.5)
# print 1+art1_der['tiempo_completo'][0, i]
ind += 1
# print art1_der['senialCompleta'][0, i]
# Create an empty trajectory goal
piernas_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
piernas_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
piernas_goal.goal_time_tolerance = rospy.Duration(0.001)
# Send the goal to the action server
arm_client.send_goal(piernas_goal)
if not sync:
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
arm_client.wait_for_result()
# arm_client.send_goal(piernas_goal)
print arm_client.get_result()
rospy.loginfo('...done')
# TODO: Add checking if action server is still alive
# and manage reconnection (I think this is not automatic)
if __name__ == '__main__':
rospy.init_node('test_jtcm')
# Launch PR2 simulation
# roslaunch pr2_gazebo pr2_empty_world.launch
jtcm = PR2JointTrajectoryControllerManager('/head_traj_controller')
rospy.sleep(2)
# Try to send a goal with all joints
jt = JointTrajectory()
jt.joint_names = ['head_pan_joint', 'head_tilt_joint']
p = JointTrajectoryPoint()
for j in jt.joint_names:
p.positions.append(0.3)
p.velocities.append(0.1)
p.time_from_start = rospy.Duration(1.0)
jt.points.append(p)
rospy.loginfo("Sending goal to both joints")
jtcm.send_goal(jt)
rospy.sleep(1.0)
# Try to send a goal with extra joints that dont pertain
from copy import deepcopy
jt2 = deepcopy(jt)
jt2.joint_names.append('non_existing_joint')
jt2.points[0].positions[0] = -0.3
#!/usr/bin/env python
import rospy, time
from rospy.rostime import Duration
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
rospy.init_node('arm_traj_loop')
left_arm_pub = rospy.Publisher('r2d2_left_arm_controller/command',
JointTrajectory,
queue_size=10)
right_arm_pub = rospy.Publisher('r2d2_right_arm_controller/command',
JointTrajectory,
queue_size=10)
waypoint1 = JointTrajectoryPoint()
waypoint1.positions = [0.25, -1.5, 0.0]
waypoint1.time_from_start = Duration(0.0)
waypoint2 = JointTrajectoryPoint()
waypoint2.positions = [0.75, -2.0, 0.75]
waypoint2.time_from_start = Duration(1.0)
waypoint3 = JointTrajectoryPoint()
waypoint3.positions = [-0.15, -3.0, -1]
waypoint3.time_from_start = Duration(3.0)
waypoint4 = JointTrajectoryPoint()
waypoint4.positions = [0, 0.5, 1]
waypoint4.time_from_start = Duration(5.0)
left_traj = JointTrajectory()
left_traj.joint_names = ["left_shoulder", "left_elbow", "left_wrist"]
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 = []
for i in range(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
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])
# Rotation matrix from the base to the end-effector
trans0_G = xyz_fixed_angle(roll, pitch, yaw)[:3, :3]
# coordinates of the spherical wrist center
dp = DG * np.matmul(trans0_G, np.array([1, 0, 0]))
px_wc = px - dp[0, 0]
py_wc = py - dp[0, 1]
pz_wc = pz - dp[0, 2]
# obtain thetas for the first three joints
tht1, tht2, tht3 = solve_position_kinematics(px_wc, py_wc, pz_wc)
trans0_3 = trans_base_link3(tht1, tht2, tht3)[:3, :3]
# The inverse matrix of R0_3 is its transpose matrix
trans3_G = np.matmul(trans0_3.T, trans0_G)
# r11 = -sin(theta5)*cos(theta4)
# r31 = sin(theta5)*sin(theta4)
tht4 = np.arctan2(trans3_G[2, 0], -trans3_G[0, 0])
# r22 = sin(theta5)*sin(theta6)
# r23 = sin(theta5)*cos(theta6)
tht6 = np.arctan2(trans3_G[1, 1], trans3_G[1, 2])
# r12 = cos(theta5)
# There are two possible solutions for theta5, but only one of them is correct
tht5 = np.arctan2(np.sqrt(1 - trans3_G[1, 0] ** 2), trans3_G[1, 0])
if np.abs(np.sin(tht5) * np.sin(tht6) - trans3_G[1, 1]) < 1e-6 and \
np.abs(np.sin(tht5) * np.cos(tht6) - trans3_G[1, 2]) < 1e-6:
pass
else:
tht5 = np.arctan2(-np.sqrt(1 - trans3_G[1, 0] ** 2), trans3_G[1, 0])
if np.abs(np.sin(tht5) * np.sin(tht6) - trans3_G[1, 1]) < 1e-6 and \
np.abs(np.sin(tht5) * np.cos(tht6) - trans3_G[1, 2]) < 1e-6:
pass
else:
print("Warning: Not found! Solution for theta5!")
# print("The solution is: \ntheta1 = {}, theta2 = {}, theta3 = {}\n"
# "theta4 = {}, theta5 = {}, theta6 = {}".
# format(tht1, tht2, tht3, tht4, tht5, tht6))
prediction = trans_base_linkg(tht1, tht2, tht3, tht4, tht5, tht6)
print("px error: {:.4e}, py error {:.4e}, pz error {:.4e}".
format(prediction[0, 3] - px, prediction[1, 3] - py, prediction[2, 3] - pz))
# Populate response for the IK request
joint_trajectory_point.positions = [
tht1, tht2, tht3, tht4, tht5, tht6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
sub_joints = rospy.Subscriber('/arm_controller/state',
JointTrajectoryControllerState, cb_arm_state)
pub_control = rospy.Publisher("/arm_controller/command",
JointTrajectory,
queue_size=1)
freq = 20 # Number of commands published per second
joint_vel_max = 1.0 # Maximum velocity of a single joint - if this is exceeded all joint velocities are scaled down
duration_mult = 2 # Velocities are set as points to reach in the future - this multiplies how far ahead to make that point. At 1 the goal is at (1/freq), causing a lot of jittery motion in the arm
r = rospy.Rate(freq)
msg_arm = JointTrajectory()
msg_arm.joint_names = [
"arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint",
"arm_6_joint", "arm_7_joint"
]
msg_arm_point = JointTrajectoryPoint()
msg_arm_point.time_from_start = Duration(nsecs=int(1e9 / freq * duration_mult))
joint_vel_command = np.ndarray(
shape=(7, 1),
dtype=float,
buffer=np.array([1, 1, 0, 0, 0, 0, 0], dtype=float) * joint_vel_max)
while not rospy.is_shutdown():
r.sleep()
if (joint_angles is None):
# Skip if not enough callbacks yet
continue
if np.all(np.equal(joint_vel_command, np.zeros(shape=(7, 1)))):
# Skip if command is 0
continue
client_server = client.wait_for_server(rospy.Duration(10.0))
if not client_server:
msg = ("Action server not available."
" Verify action server availability.")
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
# Creates a goal to send to the action server.
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ['arm_elbow_joint',\
'arm_shoulder_lift_joint', 'arm_shoulder_pan_joint',\
'arm_wrist_1_joint', 'arm_wrist_2_joint', 'arm_wrist_3_joint']
goal.trajectory.points = []
point0 = JointTrajectoryPoint()
point0.positions = [0.0,0.0,0.0,0.0,0.0,0.0]
point0.velocities = [0.0,0.0,0.0,0.0,0.0,0.0]
# To be reached 1 second after starting along the trajectory
point0.time_from_start = rospy.Duration(1.0)
goal.trajectory.points.append(point0)
point1 = JointTrajectoryPoint()
point1.positions = [0.05,0.05,0.05,0.05,0.05,0.03]
point1.velocities = [0.0,0.0,0.0,0.0,0.0,0.0]
point1.time_from_start = rospy.Duration(2.0)
goal.trajectory.points.append(point1)
# point2 = JointTrajectoryPoint()
# point2.positions = [0.0,0.0,0.0,0.0,0.0,0.2]
# point2.velocities = [0.0,0.0,0.0,0.0,0.0,0.0]
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:
start_time = time()
### Your FK code here
# Create symbols
#
# α_{i-1} for Rx
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
# a_{i-1} for Dx
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
# θi for Rz, **Joint Angles**
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# di for Dz
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
#
# Create Modified DH parameters
#
# Define dh parameters
dh_params = {
alpha0: 0,
a0: 0,
q1: q1,
d1: 0.33 + 0.42,
alpha1: -pi / 2,
a1: 0.35,
q2: q2 - pi / 2,
d2: 0,
alpha2: 0,
a2: 1.25,
q3: q3,
d3: 0,
alpha3: -pi / 2,
a3: -0.054,
q4: q4,
d4: 0.96 + 0.54,
alpha4: pi / 2,
a4: 0,
q5: q5,
d5: 0,
alpha5: -pi / 2,
a5: 0,
q6: q6,
d6: 0,
alpha6: 0,
a6: 0,
q7: 0,
d7: 0.193 + 0.11,
}
#
# Define Modified DH Transformation matrix
#
t1 = matrix_from(alpha0, a0, q1, d1).subs(dh_params)
t2 = matrix_from(alpha1, a1, q2, d2).subs(dh_params)
t3 = matrix_from(alpha2, a2, q3, d3).subs(dh_params)
t4 = matrix_from(alpha3, a3, q4, d4).subs(dh_params)
t5 = matrix_from(alpha4, a4, q5, d5).subs(dh_params)
t6 = matrix_from(alpha5, a5, q6, d6).subs(dh_params)
t7 = matrix_from(alpha6, a6, q7, d7).subs(dh_params)
#t_ee = t1 * t2 * t3 * t4 * t5 * t6 * t7
#
# Create individual transformation matrices
#
r, p, y = symbols('roll pitch yaw')
# Generate rotation matrix
rot_fix = rot_z(pi) * rot_y(-pi / 2) # Fix for URDF <-> DH-convertion
_rot_ee = rot_z(y) * rot_y(p) * rot_x(r) * rot_fix
###
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
start_each_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
])
### Your IK code here
pos_ee = Matrix([px, py, pz])
# Compensate for rotation discrepancy between DH parameters and Gazebo
rot_ee = _rot_ee.subs({r: roll, p: pitch, y: yaw})
pos_wc = pos_ee - dh_params[d7] * rot_ee[:, 2]
#
# Calculate joint angles using Geometric IK method
#
# Project the position to the plane, and thus obtain angle of (x, y)
theta1 = atan2(pos_wc[1], pos_wc[0]).evalf()
# Obtain side lengths of fig3 using pythagorean theorem
fig3_side_a = pos_wc[0] # x
fig3_side_b = pos_wc[1] # y
fig3_side_c = sqrt(fig3_side_a**2 + fig3_side_b**2)
# Obtain side lengths of fig2 using pythagorean theorem
fig2_side_a = pos_wc[2] - dh_params[d1] # z - height of joint1
fig2_side_b = fig3_side_c - dh_params[a1] # side_b-offset
fig2_side_c = sqrt(fig2_side_a**2 + fig2_side_b**2)
# Obtain side lengths of fig1 using pythagorean theorem
fig1_side_a = dh_params[d4]
fig1_side_b = fig2_side_c
fig1_side_c = dh_params[a2]
## Obtain angles of fig1
#
# Get triangle angles using sides of triangle
#
# ref.low of cosines https://en.wikipedia.org/wiki/Law_of_cosines#Applications
def angles_from_triangle_sides(a, b, c):
""":a, b, c: side values of triangle"""
gamma = acos((a**2 + b**2 - c**2) / (2 * a * b))
beta = acos((a**2 + c**2 - b**2) / (2 * a * c))
alpha = acos((b**2 + c**2 - a**2) / (2 * b * c))
return (alpha, beta, gamma)
fig1_angle_a, fig1_angle_b, _ = angles_from_triangle_sides(
fig1_side_a, fig1_side_b, fig1_side_c)
theta2 = pi / 2 - fig1_angle_a - atan2(fig2_side_a,
fig2_side_b).evalf()
theta3 = pi / 2 - fig1_angle_b + atan2(dh_params[a3],
dh_params[d4]).evalf()
#
# Extract rotation matrices from the transformation matrices
#
# Slice Rotation Matrix, i=1..3
rot0_3 = (t1 * t2 * t3)[0:3, 0:3].evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3
})
# Obtain the Rotation Matrix, i=3..6
# Memo:
# r: rotation_matrix
# inv(r) == r^T
rot3_6 = rot0_3.T * rot_ee
#_rot3_6 = rot3_6
theta4, theta5, theta6 = euler_angles_from_rotation_matrix(rot3_6)
#
###
rospy.loginfo(" Each time[%d]: %04.4f seconds" %
(x, time() - start_each_time))
# 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(" Total run time: %04.4f seconds" %
(time() - start_time))
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
T0_1,T1_2,T2_3,q1,q2,q3 = FK()
# 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
# Find EE rotation Matrix
r,p,y = symbols('r p y')
ROT_x = rot_x(r) # roll
ROT_y = rot_y(p) # pitch
ROT_z = rot_z(y) # yaw
ROT_EE = ROT_z * ROT_y * ROT_x
Rot_error = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * Rot_error #compenstate for error
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px],
[py],
[pz]])
WC = EE - (0.303) * ROT_EE[:,2] # wrist center
# calculate joint angles using Geometric IK method
theta1 = atan2(WC[1],WC[0])
# SSS triangle for theta2 and theta3
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) # sag in link 4 -0.054m
# rotation matrices
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") * ROT_EE
R3_6 = R0_3.transpose() * ROT_EE
#Eular angles from rotation matrix
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 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')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
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: q2 - pi / 2.,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2.,
a3: -0.054,
d4: 1.50,
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 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)
# Extract rotation matrices from the transformation matrices
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
# Define RPY rotation matrix
r, p, y = symbols('r p y')
ROT_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)],
[0, sin(r), cos(r)]])
ROT_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0.],
[-sin(p), 0, cos(p)]])
ROT_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0],
[0, 0, 1]])
# Compensate for rotation discrepancy between DH parameters and Gazebo
ROT_EE = ROT_z * ROT_y * ROT_x
Rot_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs(
p, radians(-90))
ROT_EE = ROT_EE * Rot_Error
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303) * ROT_EE[:, 2]
# Calculate joint angles using Geometric IK method
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") * ROT_EE
# Euler angles from rotation matrix
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 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 = []
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
])
# Calculate joint angles using Geometric IK method
lx = cos(roll) * cos(pitch)
ly = sin(roll) * cos(pitch)
lz = -sin(pitch)
gripper_length = 0.303 #d6 +d7
wx = px - (gripper_length) * lx
wy = py - (gripper_length) * ly
wz = pz - (gripper_length) * lz
q1 = atan2(wy, wx)
#q2 position
x_2 = 0.35 * cos(q1)
y_2 = 0.35 * sin(q1)
z_2 = 0.75
a2 = 1.25
d3 = 1.5
#distance q2-q4
dist1 = sqrt((wx - x_2)**2.0 + (wy - y_2)**2.0 + (wz - z_2)**2.0)
q3 = acos((a2**2.0 + d3**2.0 - dist1**2.0) / (2.0 * a2 * d3))
q3 = (q3 - pi / 2.0) * -1.0 #conversion to robot joint angle
q2 = atan2(wz - z_2, sqrt((wx - x_2)**2 + (wy - y_2)**2)) + acos(
(a2**2.0 - d3**2.0 + dist1**2.0) / (2.0 * a2 * dist1))
q2 = (q2 - pi / 2.0) * -1.0 #conversion to robot joint angle
#Roll and pitch in wrist coordinates space
roll2 = roll - q1
pitch2 = pitch - (q2 + q3)
#End efector projection in axe
dgry = 0.303 * cos(pitch2) * sin(roll2)
dgrz = 0.303 * sin(pitch2)
#wrist angles
q4 = atan2(dgry, dgrz)
q5 = atan2(sqrt(dgry**2 + dgrz**2), cos(pitch2) * cos(roll2))
#clip q5 to avoid forbbiden values
if q5 > 2.18:
q5 = 2.18
if q5 < -2.18:
q5 = -2.18
if pitch > -pi / 2 and pitch < pi / 2:
q6 = yaw - q4
else:
q6 = pi - (yaw - q4)
# Populate response for the IK request
joint_trajectory_point.positions = [q1, q2, q3, q4, q5, q6]
#rospy.loginfo("Angles: %s" % joint_trajectory_point.positions)
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
if lyap > 0.001:
phy = 2
else:
phy = 0
vel_commands = -phy*lyap_vctrl
# test_vel = np.array([-0.2, 0, 0, 0, 0, 0])
# publish controller
dt = time.time() - self.old_time
self.old_time = time.time()
target_pose = q + vel_commands*dt
print ("Vel Controller:")
print (vel_commands)
print ('Joint_Config')
print (q)
self.joint_traj.points = [JointTrajectoryPoint(positions = q,
velocities= -vel_commands,
time_from_start=rospy.Duration(0.1))]
self.sim_joint_command_pub.publish(self.joint_traj)
def scale_translation(self, X):
X[0:3, 3] = self.t_scale*X[0:3, 3]
return X
def get_best_target(self, X_H):
X_T1 = self.get_target_pose('cube1_link', 'world')
X_T2 = self.get_target_pose('cube2_link', 'world')
X_T3 = self.get_target_pose('cube3_link', 'world')
X_T_candidates = [X_T1, X_T2, X_T3]
X_Ts = []
for X_T_temp in X_T_candidates:
def open_gripper(self):
self.gripper_msg.points = [JointTrajectoryPoint(positions=[0.0], velocities=[0], time_from_start=rospy.Duration(1.0))]
self.griper_pos.publish(self.gripper_msg)
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
# link_offset_i: signed distance from x_(i-1) to x_i along z_i
# link_length_(i-1): distance from z_(i-1) to z_i along x_i, where x_i is perpendicular to both z_(i-1) and z_i
# twist_angle_(i-1): angle between z_(i-1) and z_i about x_(i-1) in the right hand sense
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # link_offset_i
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link_length_(i-1)
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')# twist_angle_(i-1)
# theta_i: joint angle between x_(i-1) and x_i about z_i in the right hand sense
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # theta_i
# 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}
# Create individual transformation matrices
T0_1 = Transformation_Matrix(q1, alpha0, a0, d1)
T0_1 = T0_1.subs(s)
T1_2 = Transformation_Matrix(q2, alpha1, a1, d2)
T1_2 = T1_2.subs(s)
T2_3 = Transformation_Matrix(q3, alpha2, a2, d3)
T2_3 = T2_3.subs(s)
T0_3 = simplify(T0_1 * T1_2 * T2_3)
# Intrinsic rotation correction for end-effector transformation matrix
R_z_intrinsic = Matrix([[ cos(pi), -sin(pi), 0],
[ sin(pi), cos(pi), 0],
[ 0, 0, 1]])
R_y_intrinsic = Matrix([[ cos(-pi/2), 0, sin(-pi/2)],
[ 0, 1, 0],
[-sin(-pi/2), 0, cos(-pi/2)]])
ZY_intrinsic_rot = R_z_intrinsic * R_y_intrinsic
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])
# Calculate transformation matrix from base link to end-effector
R_z_extrinsic = Matrix([[ cos(yaw), -sin(yaw), 0],
[ sin(yaw), cos(yaw), 0],
[ 0, 0, 1]])
R_y_extrinsic = Matrix([[ cos(pitch), 0, sin(pitch)],
[ 0, 1, 0],
[-sin(pitch), 0, cos(pitch)]])
R_x_extrinsic = Matrix([[ 1, 0, 0],
[ 0, cos(roll), -sin(roll)],
[ 0, sin(roll), cos(roll)]])
XYZ_extrinsic_rot = R_z_extrinsic * R_y_extrinsic * R_x_extrinsic
R0_6 = XYZ_extrinsic_rot * ZY_intrinsic_rot
# Define dictionary parameters for further use
a_3 = s[a3]
d_4 = s[d4]
d_1 = s[d1]
a_1 = s[a1]
a_2 = s[a2]
d_7 = s[d7]
# Find Wrist Center Location
wx = px - (d_7 * R0_6[0, 2])
wy = py - (d_7 * R0_6[1, 2])
wz = pz - (d_7 * R0_6[2, 2])
# Finding theta 1-3
theta1 = (atan2(wy, wx)).evalf()
s1 = sqrt(wx**2 + wy**2) - a_1
s2 = wz - d_1
s3 = sqrt(s2**2 + s1**2)
s4 = sqrt(a_3**2 + d_4**2)
beta1 = atan2(s2, s1)
D2 = (a_2**2 + s3**2 - s4**2) / (2 * a_2 * s3)
beta2 = atan2(sqrt(1 - D2**2), D2)
D3 = (a_2**2 + s4**2 - s3**2) / (2 * a_2 * s4)
beta3 = atan2(sqrt(1 - D3**2), D3)
beta4 = atan2(-a_3, d_4)
theta2 = ((pi / 2) - beta2 - beta1).evalf()
theta3 = ((pi / 2) - beta4 - beta3).evalf()
# Finding theta 4-6
R0_3 = T0_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R0_3_inv = R0_3.transpose()
R3_6 = R0_3_inv * R0_6
r13 = R3_6[0, 2]
r33 = R3_6[2, 2]
r23 = R3_6[1, 2]
r21 = R3_6[1, 0]
r22 = R3_6[1, 1]
r12 = R3_6[0, 1]
r32 = R3_6[2, 1]
theta5 = (atan2(sqrt(r13**2 + r33**2), r23)).evalf()
theta4 = (atan2(r33, -r13)).evalf()
theta6 = (atan2(-r22, r21)).evalf()
print('wx: ' + str(wx))
print('wy: ' + str(wy))
print('wz: ' + str(wz))
print('theta1: ' + str(theta1))
print('theta2: ' + str(theta2))
print('theta3: ' + str(theta3))
print('theta4: ' + str(theta4))
print('theta5: ' + str(theta5))
print('theta6: ' + str(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
# link lengths
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
# link offsets
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
# Twist Angles(Alpha)
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
rospy.loginfo("Created DH parameter symbols")
# Joint angle symbols(Theta)
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
rospy.loginfo("Created Joint Angle symbols")
# Create Modified DH parameters
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}
rospy.loginfo("Created DH parameters")
# Define Modified DH Transformation matrix
# Create individual transformation matrices
t0_1 = dh_transformation(alpha0, a0, q1, d1).subs(s)
t1_2 = dh_transformation(alpha1, a1, q2, d2).subs(s)
t2_3 = dh_transformation(alpha2, a2, q3, d3).subs(s)
t3_4 = dh_transformation(alpha3, a3, q4, d4).subs(s)
t4_5 = dh_transformation(alpha4, a4, q5, d5).subs(s)
t5_6 = dh_transformation(alpha5, a5, q6, d6).subs(s)
t6_7 = dh_transformation(alpha6, a6, q7, d7).subs(s)
t0_7 = simplify(t0_1 * t1_2 * t2_3 * t3_4 * t4_5 * t5_6 * t6_7)
rospy.loginfo("Created individuals transformation matrices")
# Extract rotation matrices from the transformation matrices
rot_corr = corr_matrix()
calc_fwd_kin = simplify(t0_7 * rot_corr)
t0_3 = simplify(t0_1 * t1_2 * t2_3)
rot_0_3 = t0_3[0:3, 0:3]
rospy.loginfo("Extracted rot matrices and calculated FK")
# 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 joint angles using Geometric IK method
roll, pitch, yaw = calculate_rot_rpw(roll, pitch, yaw)
rot_0_6 = simplify(roll * pitch * yaw)
p_end_effector = Matrix([[px], [py], [pz]])
joint5 = calc_wrist_center(p_end_effector, rot_0_6)
theta1 = calc_theta1(joint5)
theta3_intern, theta3 = calc_theta3(joint5, theta1)
joint2 = get_joint2(theta1)
theta2 = calc_theta2(joint2, joint5, theta3_intern)
# Compensate for rotation discrepancy between DH parameters and Gazebo
rot_0_3_matrix = rot_0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
rot_0_3_matrix_inverse = rot_0_3_matrix ** -1
rot_3_6 = rot_0_3_matrix_inverse * rot_0_6
theta6 = calc_theta6(rot_3_6)
theta5 = calc_theta5(rot_3_6)
theta4 = calc_theta4(rot_3_6)
###
# 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)
forward_kinematics = calc_fwd_kin.evalf(
subs={q1: theta1, q2: theta2, q3: theta3, q4: theta4, q5: theta5, q6: theta6})
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def __init__(self):
pi4 = 0.7853
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_robot_xacro4_cambio.urdf'
robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")
print cadena_der_up_down.getNrOfSegments()
fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down)
fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up)
fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down)
fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up)
vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down)
vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up)
vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down)
vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up)
nj_izq = cadena_der_up_down.getNrOfJoints()
posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_der_up_down[0] = 0
posicionInicial_der_up_down[1] = 0.3
posicionInicial_der_up_down[2] = -0.3
posicionInicial_der_up_down[3] = 0.6
posicionInicial_der_up_down[4] = -0.3
posicionInicial_der_up_down[5] = -0.3
nj_izq = cadena_izq_up_down.getNrOfJoints()
posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_izq_up_down[0] = 0
posicionInicial_izq_up_down[1] = 0.3
posicionInicial_izq_up_down[2] = -0.3
posicionInicial_izq_up_down[3] = 0.6
posicionInicial_izq_up_down[4] = -0.3
posicionInicial_izq_up_down[5] = -0.3
nj_izq = cadena_der_down_up.getNrOfJoints()
posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_der_down_up[5] = 0
posicionInicial_der_down_up[4] = 0.3
posicionInicial_der_down_up[3] = -0.3
posicionInicial_der_down_up[2] = 0.6
posicionInicial_der_down_up[1] = -0.3
posicionInicial_der_down_up[0] = -0.3
nj_izq = cadena_izq_down_up.getNrOfJoints()
posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_izq_down_up[5] = 0
posicionInicial_izq_down_up[4] = 0.3
posicionInicial_izq_down_up[3] = -0.3
posicionInicial_izq_down_up[2] = 0.6
posicionInicial_izq_down_up[1] = -0.3
posicionInicial_izq_down_up[0] = -0.3
print "Forward kinematics"
finalFrame_izq_up_down = PyKDL.Frame()
finalFrame_izq_down_up = PyKDL.Frame()
finalFrame_der_up_down = PyKDL.Frame()
finalFrame_der_down_up = PyKDL.Frame()
print "Rotational Matrix of the final Frame: "
print finalFrame_izq_up_down.M
print "End-effector position: ", finalFrame_izq_up_down.p
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
piernas_joints = ['cilinder_blue_box1_der_joint', 'cilinder_blue1_box2_der_joint','cilinder_blue_box2_der_joint',
'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint','cilinder_blue_box6_der_joint',
'cilinder_blue_box1_izq_joint', 'cilinder_blue1_box2_izq_joint','cilinder_blue_box2_izq_joint',
'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint','cilinder_blue_box6_izq_joint']
piernas_goal0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#11111111111111111111111111111111111111111111
print "Inverse Kinematics"
fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down)
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
q_init_izq_up_down = posicionInicial_izq_up_down # initial angles
desiredFrame = finalFrame_izq_up_down
desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
print "Desired Position: ", desiredFrame.p
q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
print "Output angles in rads: ", q_out_izq_up_down
print "Inverse Kinematics"
fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down)
fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
q_init_der_up_down = posicionInicial_der_up_down # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame = finalFrame_der_up_down
desiredFrame.p[0] = finalFrame_der_up_down.p[0]
desiredFrame.p[1] = finalFrame_der_up_down.p[1]
desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02 #0.02
print "Desired Position: ", desiredFrame.p
q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down)
print "Output angles in rads: ", q_out_der_up_down
piernas_goal0[6] = q_out_izq_up_down[0]
piernas_goal0[7] = q_out_izq_up_down[1]
piernas_goal0[8] = q_out_izq_up_down[2]
piernas_goal0[9] = q_out_izq_up_down[3]
piernas_goal0[10] = q_out_izq_up_down[4]
piernas_goal0[11] = q_out_izq_up_down[5]
piernas_goal0[0] = q_out_der_up_down[0]
piernas_goal0[1] = q_out_der_up_down[1]
piernas_goal0[2] = q_out_der_up_down[2]
piernas_goal0[3] = q_out_der_up_down[3]
piernas_goal0[4] = q_out_der_up_down[4]
piernas_goal0[5] = q_out_der_up_down[5]
#121212122121212121212121212121212121212121212
piernas_goal12 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#q_out_izq_up_down[0] -= pi4
piernas_goal12[6] = q_out_izq_up_down[0]-2*pi4
piernas_goal12[7] = q_out_izq_up_down[1]
piernas_goal12[8] = q_out_izq_up_down[2]
piernas_goal12[9] = q_out_izq_up_down[3]
piernas_goal12[10] = q_out_izq_up_down[4]
piernas_goal12[11] = q_out_izq_up_down[5]
piernas_goal12[0] = 0
piernas_goal12[1] = 0
piernas_goal12[2] = -0.7
piernas_goal12[3] = 1.4
piernas_goal12[4] = 0
piernas_goal12[5] = -0.7
#########################################################
piernas_goal2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
piernas_goal2[6] = q_out_izq_up_down[0]-2*pi4
piernas_goal2[7] = q_out_izq_up_down[1]-0.3-0.3
piernas_goal2[8] = q_out_izq_up_down[2]-0.3
piernas_goal2[9] = q_out_izq_up_down[3]+0.6
piernas_goal2[10] = q_out_izq_up_down[4]+0.3+0.3
piernas_goal2[11] = q_out_izq_up_down[5] -0.3
piernas_goal2[0] = 0#-pi4
piernas_goal2[1] = -0.25
piernas_goal2[2] = -0.3
piernas_goal2[3] = 0.6
piernas_goal2[4] = 0.25
piernas_goal2[5] = -0.3
##################################################
piernas_goal3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
piernas_goal3[6] = q_out_izq_up_down[0] +0.0
piernas_goal3[7] = q_out_izq_up_down[1] - 0.3
piernas_goal3[8] = q_out_izq_up_down[2] - 0.3
piernas_goal3[9] = q_out_izq_up_down[3] + 0.6
piernas_goal3[10] = q_out_izq_up_down[4] + 0.3
piernas_goal3[11] = q_out_izq_up_down[5] - 0.3
piernas_goal3[0] = -2*pi4
piernas_goal3[1] = -0.25
piernas_goal3[2] = -0.3
piernas_goal3[3] = 0.6
piernas_goal3[4] = 0.25
piernas_goal3[5] = -0.3
##################################################
piernas_goal4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
piernas_goal4[6] = q_out_izq_up_down[0] +0.0
piernas_goal4[7] = q_out_izq_up_down[1] - 0.3 +0.3
piernas_goal4[8] = q_out_izq_up_down[2] - 0.3 +0.3
piernas_goal4[9] = q_out_izq_up_down[3] + 0.6 -0.6
piernas_goal4[10] = q_out_izq_up_down[4] + 0.3 -0.3
piernas_goal4[11] = q_out_izq_up_down[5] - 0.3 +0.3
piernas_goal4[0] = -2*pi4
piernas_goal4[1] = -0.25
piernas_goal4[2] = -0.7
piernas_goal4[3] = 1.4
piernas_goal4[4] = 0.25
piernas_goal4[5] = -0.7
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
#/piernas/piernas_controller/follow_joint_trajectory
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the piernas_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = piernas_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = piernas_goal0
arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[1].positions = piernas_goal12
arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[1].time_from_start = rospy.Duration(6.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[2].positions = piernas_goal2
arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[2].time_from_start = rospy.Duration(9.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[3].positions = piernas_goal3
arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[3].time_from_start = rospy.Duration(12.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[4].positions = piernas_goal4
arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[4].time_from_start = rospy.Duration(15.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[5].positions = piernas_goal12
arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[5].time_from_start = rospy.Duration(18.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[6].positions = piernas_goal2
arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[6].time_from_start = rospy.Duration(21.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[7].positions = piernas_goal3
arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[7].time_from_start = rospy.Duration(24)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[8].positions = piernas_goal4
arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[8].time_from_start = rospy.Duration(27)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[9].positions = piernas_goal12
arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[9].time_from_start = rospy.Duration(30.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[10].positions = piernas_goal2
arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[10].time_from_start = rospy.Duration(33.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[11].positions = piernas_goal3
arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[11].time_from_start = rospy.Duration(36.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[12].positions = piernas_goal4
arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[12].time_from_start = rospy.Duration(39.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[13].positions = piernas_goal12
arm_trajectory.points[13].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[13].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[13].time_from_start = rospy.Duration(42.0)
'''arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[14].positions = piernas_goal4
arm_trajectory.points[14].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[14].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[14].time_from_start = rospy.Duration(32.0)'''
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
piernas_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
piernas_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
piernas_goal.goal_time_tolerance = rospy.Duration(0.01)
# Send the goal to the action server
arm_client.send_goal(piernas_goal)
if not sync:
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
arm_client.wait_for_result()
print arm_client.get_result()
rospy.loginfo('...done')
def traj_planner(self,
cart_pos,
grasp_step='move',
way_points_number=10,
movement='slow'):
"""Quintic Trajectory Planner
Publish a trajectory to UR5 using quintic splines.
Arguments:
cart_pos {[float]} -- Grasp position [x, y, z]
Keyword Arguments:
grasp_step {str} -- Set UR5 movement type (default: {'move'})
way_points_number {number} -- Number of points considered in trajectory (default: {10})
movement {str} -- Movement speed (default: {'slow'})
"""
offset_from_object = 0.05
if grasp_step == 'pregrasp':
self.grasp_cartesian_pose = deepcopy(self.posCB)
self.grasp_cartesian_pose[-1] += offset_from_object
joint_pos = self.get_ik(self.grasp_cartesian_pose)
if joint_pos is not None:
joint_pos[-1] = self.ori
self.final_orientation = deepcopy(self.ori)
self.gripper_angle_grasp = deepcopy(self.d[-2])
elif grasp_step == 'grasp':
self.grasp_cartesian_pose[-1] -= offset_from_object
joint_pos = self.get_ik(self.grasp_cartesian_pose)
if joint_pos is not None:
joint_pos[-1] = self.final_orientation
elif grasp_step == 'move':
joint_pos = self.get_ik(cart_pos)
if joint_pos is not None:
joint_pos[-1] = 0.0
if joint_pos is not None:
if movement == 'slow':
final_traj_duration = 500.0 # total iteractions
elif movement == 'fast':
final_traj_duration = 350.0
v0 = a0 = vf = af = 0
t0 = 5.0
tf = (t0 +
final_traj_duration) / way_points_number # tf by way point
t = tf / 10 # for each movement
ta = tf / 10 # to complete each movement
a = [0.0] * 6
pos_points, vel_points, acc_points = [0.0] * 6, [0.0] * 6, [0.0
] * 6
goal = self.__build_goal_message_ur5()
for i in range(6):
q0 = self.actual_position[i]
qf = joint_pos[i]
b = np.array([q0, v0, a0, qf, vf, af]).transpose()
m = np.array([[1, t0, t0**2, t0**3, t0**4, t0**5],
[0, 1, 2 * t0, 3 * t0**2, 4 * t0**3, 5 * t0**4],
[0, 0, 2, 6 * t0, 12 * t0**2, 20 * t0**3],
[1, tf, tf**2, tf**3, tf**4, tf**5],
[0, 1, 2 * tf, 3 * tf**2, 4 * tf**3, 5 * tf**4],
[0, 0, 2, 6 * tf, 12 * tf**2, 20 * tf**3]])
a[i] = np.linalg.inv(m).dot(b)
for i in range(way_points_number):
for j in range(6):
pos_points[j] = a[j][0] + a[j][1] * t + a[j][2] * t**2 + a[
j][3] * t**3 + a[j][4] * t**4 + a[j][5] * t**5
vel_points[j] = a[j][1] + 2 * a[j][2] * t + 3 * a[j][
3] * t**2 + 4 * a[j][4] * t**3 + 5 * a[j][5] * t**4
acc_points[j] = 2 * a[j][2] + 6 * a[j][3] * t + 12 * a[j][
4] * t**2 + 20 * a[j][5] * t**3
goal.trajectory.points.append(
JointTrajectoryPoint(
positions=pos_points,
velocities=vel_points,
accelerations=acc_points,
time_from_start=rospy.Duration(t))) #default 0.1*i + 5
t += ta
self.client.send_goal(goal)
self.all_close(joint_pos)
else:
print('Could not find a IK solution')
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()
