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 readInPoses(self):
poses = rospy.get_param('~poses')
rospy.loginfo("Found %d poses on the param server", len(poses))
for key,value in poses.iteritems():
try:
# TODO: handle multiple points in trajectory
trajectory = JointTrajectory()
trajectory.joint_names = value["joint_names"]
point = JointTrajectoryPoint()
point.time_from_start = Duration(value["time_from_start"])
point.positions = value["positions"]
trajectory.points = [point]
self.poseLibrary[key] = trajectory
except:
rospy.logwarn("Could not parse pose \"%s\" from the param server:", key);
rospy.logwarn(sys.exc_info())
# add a default crouching pose:
if not "crouch" in self.poseLibrary:
trajectory = JointTrajectory()
trajectory.joint_names = ["Body"]
point = JointTrajectoryPoint()
point.time_from_start = Duration(1.5)
point.positions = [0.0,0.0, # head
1.545, 0.33, -1.57, -0.486, 0.0, 0.0, # left arm
-0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # left leg
-0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # right leg
1.545, -0.33, 1.57, 0.486, 0.0, 0.0] # right arm
trajectory.points = [point]
self.poseLibrary["crouch"] = trajectory;
def talker():
if 1 == 0:
pub = rospy.Publisher("/ihmc_ros/atlas/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
jn = ["l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx", "l_arm_wry2"]
jn_r = ["r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx", "r_arm_wry2"]
else:
pub = rospy.Publisher("/ihmc_ros/valkyrie/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
jn = [
"LeftShoulderPitch",
"LeftShoulderRoll",
"LeftShoulderYaw",
"LeftElbowPitch",
"LeftForearmYaw",
"LeftWristRoll",
"LeftWristPitch",
]
jn_r = [
"RightShoulderPitch",
"RightShoulderRoll",
"RightShoulderYaw",
"RightElbowPitch",
"RightForearmYaw",
"RightWristRoll",
"RightWristPitch",
]
# this doesnt work:
# jn = ["l_arm_shz","l_arm_shx","l_arm_ely","l_arm_elx","l_arm_wry","l_arm_wrx","l_arm_wry2", "r_arm_shz","l_arm_shx","r_arm_ely","r_arm_elx","r_arm_wry","r_arm_wrx","r_arm_wry2"]
# value = 0
rospy.init_node("send_arm_test", anonymous=True)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
msg = JointTrajectory()
value = 0.1
value_r = 0.1
msg.joint_names = jn
pt = JointTrajectoryPoint()
pt.positions = [value] * len(jn)
pt.velocities = [0] * len(jn)
pt.accelerations = [0] * len(jn)
pt.effort = [0] * len(jn)
pt.time_from_start = rospy.Duration.from_sec(3)
msg.points.append(pt)
print msg.joint_names, rospy.get_time()
pub.publish(msg)
# TODO: add a sleep here between left and right
msg.joint_names = jn_r
msg.points[0].positions = [value_r] * len(jn)
print msg.joint_names, rospy.get_time()
pub.publish(msg)
rate.sleep()
def create_hand_goal(self, closing_amount):
jt = JointTrajectory()
if self.robot == "reemc":
jt.joint_names = ['hand_right_thumb_joint', 'hand_right_index_joint', 'hand_right_mrl_joint']
if self.robot == "tiago":
jt.joint_names = ['hand_thumb_joint', 'hand_index_joint', 'hand_mrl_joint']
jtp = JointTrajectoryPoint()
# Hardcoded joint limits
jtp.positions = [closing_amount * 6.0, closing_amount * 6.0, closing_amount * 9.0]
jtp.time_from_start = rospy.Duration(0.5)
jt.points.append(jtp)
return jt
def publishThumbPosition(self, hand, position):
'''
Publish for single value (thumb)
'''
jtm = JointTrajectory()
jtp = JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(1.0)
jtp.positions = [position]
jtm.points = [jtp]
if hand == MekaControllerConverter.RIGHT_HAND:
jtm.joint_names = ['right_hand_j0']
else:
jtm.joint_names = ['left_hand_j0']
self.hand_thumb_publishers[hand].publish(jtm)
def send_command(self, cmd):
rospy.logdebug(" Sending cmd to controller [%s]"%self._controller_id)
cmd_msg = JointTrajectory()
cmd_msg.header.stamp = rospy.Time().now()
cmd_msg.joint_names = self._config["joint_names"]
cmd_msg.points = [self._build_segment(x) for x in cmd["segments"]]
self._pub.publish(cmd_msg)
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 main():
pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory)
rospy.init_node('traj_test')
p1 = JointTrajectoryPoint()
p1.positions = [0,0,0,0,0,0,0]
p1.velocities = [0,0,0,0,0,0,0]
p1.accelerations = [0,0,0,0,0,0,0]
p2 = JointTrajectoryPoint()
p2.positions = [0,0,1.0,0,-1.0,0,0]
p2.velocities = [0,0,0,0,0,0,0]
p2.accelerations = [0,0,0,0,0,0,0]
p2.time_from_start = rospy.Time(4.0)
p3 = JointTrajectoryPoint()
p3.positions = [0,0,-1.0,0,1.0,0,0]
p3.velocities = [0,0,0,0,0,0,0]
p3.accelerations = [0,0,0,0,0,0,0]
p3.time_from_start = rospy.Time(20.0)
traj = JointTrajectory()
traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7']
traj.points = [p1,p2,p3]
rospy.loginfo(traj)
r = rospy.Rate(1) # 10hz
pub.publish(traj)
r.sleep()
pub.publish(traj)
def send_arm_goal(self, arm_goal):
arm_joint_names = ['joint_lift',
'joint_waist',
'joint_big_arm',
'joint_forearm',
'joint_wrist_pitching',
'joint_wrist_rotation']
rospy.loginfo("Waiting for follow_joint_trajectory server")
self.arm_client.wait_for_server()
rospy.loginfo("Connected to follow_joint_trajectory server")
rospy.sleep(1)
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joint_names
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].time_from_start = rospy.Duration(10)
rospy.loginfo("Preparing for moving the arm to goal position!")
rospy.sleep(1)
arm_goal_pos = FollowJointTrajectoryGoal()
arm_goal_pos.trajectory = arm_trajectory
arm_goal_pos.goal_time_tolerance = rospy.Duration(0)
self.arm_client.send_goal(arm_goal_pos)
rospy.loginfo("Send goal to the trajectory server successfully!")
self.arm_client.wait_for_result()
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 __create_spin_command(self, arm, theta=math.pi):
if arm == 'l':
jnts = self.robot_state.left_arm_positions
if arm == 'r':
jnts = self.robot_state.right_arm_positions
command = JointTrajectory()
command.joint_names = ['%s_shoulder_pan_joint' % arm[0],
'%s_shoulder_lift_joint' % arm[0],
'%s_upper_arm_roll_joint' % arm[0],
'%s_elbow_flex_joint' % arm[0],
'%s_forearm_roll_joint' % arm[0],
'%s_wrist_flex_joint' % arm[0],
'%s_wrist_roll_joint' % arm[0]]
jnts[-1] += theta
command.points.append(JointTrajectoryPoint(
positions=jnts,
velocities = [0.0] * (len(command.joint_names)),
accelerations = [],
time_from_start = rospy.Duration(0.1)))
goal = JointTrajectoryGoal()
goal.trajectory = command
return goal
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 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 move_head(self, pan , tilt):
# Which joints define the head?
head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
# Create a single-point head trajectory with the head_goal as the end-point
head_trajectory = JointTrajectory()
head_trajectory.joint_names = head_joints
head_trajectory.points.append(JointTrajectoryPoint())
head_trajectory.points[0].positions = pan , tilt
head_trajectory.points[0].velocities = [0.0 for i in head_joints]
head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = head_trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
self.head_client.send_goal(head_goal)
# Wait for up to 5 seconds for the motion to complete
self.head_client.wait_for_result(rospy.Duration(2.0))
rospy.loginfo('...done')
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 exectute_joint_traj(self, joint_trajectory, timing):
'''Moves the arm through the joint sequence'''
# First, do filtering on the trajectory to fix the velocities
trajectory = JointTrajectory()
# Initialize the server
# When to start the trajectory: 0.1 seconds from now
trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
trajectory.joint_names = self.joint_names
## Add all frames of the trajectory as way points
for i in range(len(timing)):
positions = joint_trajectory[i].joint_pose
velocities = [0] * len(positions)
# Add frames to the trajectory
trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities,
time_from_start=timing[i]))
output = self.filter_service(trajectory=trajectory,
allowed_time=rospy.Duration.from_sec(20))
rospy.loginfo('Trajectory for arm ' + str(self.arm_index) +
' has been filtered.')
traj_goal = JointTrajectoryGoal()
# TO-DO: check output.error_code
traj_goal.trajectory = output.trajectory
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.joint_names = self.joint_names
# Sends the goal to the trajectory server
self.traj_action_client.send_goal(traj_goal)
def dual_arm_test():
pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory)
cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command', JointTrajectory)
rospy.init_node('dual_arm_test')
rospy.sleep(1.0)
while not rospy.is_shutdown():
traj = JointTrajectory()
traj.joint_names = ['lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint', 'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint']
ptn = JointTrajectoryPoint()
ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3]
ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.points = []
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
def fill_ik_traj(self, poses, dist=None, duration=None, tot_points=None):
if dist is None:
dist = self.calc_dist(poses[0], poses[-1])
if duration is None:
duration = dist*30
if tot_points is None:
tot_points = 500*dist
ik_fracs = np.linspace(0, 1, len(poses)) #A list of fractional positions along course to evaluate ik
req = self.form_ik_request(poses[0])
ik_goal = self.ik_pose_proxy(req)
seed = ik_goal.solution.joint_state.position
ik_points = [[0]*7 for i in range(len(poses))]
for i, p in enumerate(poses):
request = self.form_ik_request(p)
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
ik_points[i] = ik_goal.solution.joint_state.position
seed = ik_goal.solution.joint_state.position # seed the next ik w/previous points results
try:
ik_points = np.array(ik_points)
except:
print "Value error"
print 'ik_points: ', ik_points
print 'ik_points_array: ', ik_points
ang_fracs = np.linspace(0,1, tot_points)
diff = np.max(np.abs(np.diff(ik_points,1,0)),0)
print 'diff: ', diff
for i in xrange(len(ik_points)):
if ik_points[i,4]<0.:
ik_points[i,4] += 2*math.pi
if any(ik_points[:,4]>3.8):
print "OVER JOINT LIMITS!"
ik_points[:,4] -= 2*math.pi
# Linearly interpolate angles between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path. Used to maintain large number of trajectory points without running IK on every one.
angle_points = np.zeros((7, tot_points))
for i in xrange(7):
angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i])
#Build Joint Trajectory from angle positions
traj = JointTrajectory()
traj.header.frame_id = poses[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
#print 'angle_points', len(angle_points[0]), angle_points
for i in range(len(angle_points[0])):
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = angle_points[:,i]
traj.points[i].velocities = [0]*7
traj.points[i].time_from_start = rospy.Duration(ang_fracs[i]*duration)
angles_safe = self.check_trajectory(traj)
if angles_safe:
print "Check Passed: Angles Safe"
return traj
else:
print "Check Failed: Unsafe Angles"
return None
def __init__(self):
rospy.init_node('arm_simple_trajectory')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Which joints define the arm?
arm_joints = ['joint_lift',
'joint_waist',
'joint_big_arm',
'joint_forearm',
'joint_wrist_pitching',
'joint_wrist_rotation']
if reset:
# Set the arm back to the resting position
arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
else:
# Set a goal configuration for the arm
arm_goal = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for xm arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('xm_arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
rospy.sleep(1)
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
rospy.sleep(1)
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
rospy.loginfo('...done')
rospy.sleep(1)
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 make_gripper_posture(self, pose):
t = JointTrajectory()
t.joint_names = GRIPPER_JOINT_NAMES
tp = JointTrajectoryPoint()
tp.positions = [pose for j in t.joint_names]
tp.effort = GRIPPER_EFFORT
t.points.append(tp)
return t
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 up_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = [1.]*len(self.joint_names)
jtp.velocities = [0.]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(5.0)
jtm.points = [jtp]
return jtm
def up_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = [self.filtered_angle]*len(self.joint_names)
print self.controller, self.filtered_angle
jtp.time_from_start = rospy.Duration(1.0)
jtm.points = [jtp]
return jtm
def get_post_place_posture():
pre_grasp_posture = JointTrajectory()
pre_grasp_posture.header.frame_id = END_EFFECTOR
pre_grasp_posture.header.stamp = rospy.Time.now()
pre_grasp_posture.joint_names = GRIPPER_JOINTS
pos = JointTrajectoryPoint() # open gripper
pos.positions = [0.0]
pre_grasp_posture.points.append(pos)
return pre_grasp_posture
def random_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = 2*np.random.random(len(self.joint_names)) - 1
jtp.velocities = [0.]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(5.)
jtm.points = [jtp]
return jtm
def get_post_place_posture():
post_place_posture = JointTrajectory()
post_place_posture.header.frame_id = EEF_LINK
post_place_posture.header.stamp = rospy.Time.now()
post_place_posture.joint_names = GRIPPER_JOINTS
pos = JointTrajectoryPoint() # gripper open after place
pos.positions = [0.0]
post_place_posture.points.append(pos)
return post_place_posture
def __init__(self):
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Which joints define the arm?
arm_joints = ['link_1_joint',
'servo_2_joint',
'link_3_joint',
'link_4_joint',
'link_5_joint']
if reset:
# Set the arm back to the resting position
arm_goal = [0, 0, 0, 0, 0]
else:
# Set a goal configuration for the arm
arm_goal = [-0.525487, 0.904972, 0.480017, -1.331166, 0.731413]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
rospy.loginfo('...done')
def make_joint_trajectory(names, points):
jt = JointTrajectory()
jt.joint_names = names
pt = JointTrajectoryPoint()
pt.positions = points
pt.effort = [0]*len(points)
pt.velocities = [0]*len(points)
pt.accelerations = [0]*len(points)
jt.points = [pt]
return jt
def make_gripper_posture(self, joint_positions):
# Initialize the joint trajectory for the gripper joints
t = JointTrajectory()
# Set the joint names to the gripper joint names
t.joint_names = GRIPPER_JOINT_NAMES
# Initialize a joint trajectory point to represent the goal
tp = JointTrajectoryPoint()
# Assign the trajectory joint positions to the input positions
tp.positions = joint_positions
# Set the gripper effort
tp.effort = GRIPPER_EFFORT
tp.time_from_start = rospy.Duration(1.0)
# Append the goal point to the trajectory points
t.points.append(tp)
# Return the joint trajectory
return t
def extract_part_trajectory(joint_names, trajectory):
"""
Returns a copy of the given trajectory that is reduced to the given set of joints.
:param joint_names: list of joints to keep.
:param trajectory: input trajectory
:return: a copy of the input trajectory that only contains the joints defined in joint_names
"""
output_traj = JointTrajectory()
output_traj.header = trajectory.header
indices = []
num_joints = len(trajectory.joint_names)
for name_idx in range(num_joints):
if trajectory.joint_names[name_idx] in joint_names:
indices.append(name_idx)
output_traj.joint_names = [trajectory.joint_names[i] for i in range(num_joints) if i in indices]
for point in trajectory.points:
new_point = JointTrajectoryPoint()
new_point.positions = [point.positions[i] for i in range(len(point.positions)) if i in indices]
new_point.velocities = [point.velocities[i] for i in range(len(point.velocities)) if i in indices]
new_point.accelerations = [point.accelerations[i] for i in range(len(point.accelerations)) if i in indices]
new_point.time_from_start = point.time_from_start
output_traj.points.append(new_point)
return output_traj
def move_arm_joints_v2(client, arm_joint_positions_list):
trajectory = JointTrajectory()
trajectory.joint_names = ARM_JOINT_NAMES
duration = 0.0
wait_time = 3.0
for i, a_joint_pose in enumerate(arm_joint_positions_list):
duration += wait_time
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[i].positions = a_joint_pose
trajectory.points[i].velocities = [0.0] * len(a_joint_pose)
trajectory.points[i].accelerations = [0.0] * len(a_joint_pose)
trajectory.points[i].time_from_start = rospy.Duration(duration)
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
print("Moving amr to joints: ", arm_joint_positions_list)
client.send_goal(arm_goal)
client.wait_for_result(
rospy.Duration(len(arm_joint_positions_list) * wait_time))
rospy.loginfo("...done")
def timer_callback(self):
if self.starting_point_ok:
traj = JointTrajectory()
traj.joint_names = self.joints
point = JointTrajectoryPoint()
point.positions = self.goals[self.i]
point.time_from_start = Duration(sec=4)
traj.points.append(point)
self.publisher_.publish(traj)
self.i += 1
self.i %= len(self.goals)
elif self.check_starting_point and not self.joint_state_msg_received:
self.get_logger().warn(
'Start configuration could not be checked! Check "joint_state" topic!'
)
else:
self.get_logger().warn(
"Start configuration is not within configured limits!")
def massage_traj(traj_in): traj_out = JointTrajectory() traj_out.joint_names = traj_in.joint_names npts = len(traj_in.points) # take first point traj_out.points.append(copy.deepcopy(traj_in.points[0])) traj_out.points[0].velocities = [0,0,0,0,0] traj_out.points[0].accelerations = [0,0,0,0,0] # take middle point # midpt = int(npts/2) # traj_out.points.append(copy.deepcopy(traj_in.points[midpt])) # traj_out.points[1].velocities = [0,0,0,0,0] # traj_out.points[1].accelerations = [0,0,0,0,0] # take last point traj_out.points.append(copy.deepcopy(traj_in.points[-1])) traj_out.points[-1].velocities = [0,0,0,0,0] traj_out.points[-1].accelerations = [0,0,0,0,0] print "Points in traj: " + str(len(traj_out.points)) return traj_out
def get_trajectory_message(self, action, robot_id=0):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
# Set up a trajectory message to publish.
action_msg = JointTrajectory()
action_msg.joint_names = self.environment['joint_order']
# Create a point to tell the robot to move to.
target = JointTrajectoryPoint()
action_float = [float(i) for i in action]
target.positions = action_float
# These times determine the speed at which the robot moves:
# it tries to reach the specified target position in 'slowness' time.
# target.time_from_start.secs = self.environment['slowness']
target.time_from_start.nsecs = self.environment['slowness']
# Package the single point into a trajectory of points with length 1.
action_msg.points = [target]
return action_msg
def talker():
rospy.init_node('joint_trajectory_publisher', anonymous=True)
pub = rospy.Publisher('arm_robot_trajectory_controller/command',
JointTrajectory,
queue_size=10)
rospy.sleep(0.5)
msg = JointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.joint_names = [
"arm1_joint", "arm2_joint", "arm3_joint", "arm4_joint", "arm5_joint",
"arm6_joint"
]
msg.points = [JointTrajectoryPoint() for i in range(6)]
msg.points[0].positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg.points[0].time_from_start = rospy.Time(1.0)
msg.points[1].positions = [
0.0, math.pi / 6.0, math.pi / 6.0, math.pi / 6.0, 0.0, 0.0
]
msg.points[1].time_from_start = rospy.Time(2.0)
msg.points[2].positions = [
0.5, math.pi / 6.0, math.pi / 6.0, math.pi / 6.0, 0.5, 0.0
]
msg.points[2].time_from_start = rospy.Time(3.0)
msg.points[3].positions = [
-0.5, math.pi / 6.0, math.pi / 6.0, math.pi / 6.0, -0.5, 0.0
]
msg.points[3].time_from_start = rospy.Time(4.0)
msg.points[4].positions = [
0.0, math.pi / 6.0, math.pi / 6.0, math.pi / 6.0, 0.0, 0.0
]
msg.points[4].time_from_start = rospy.Time(5.0)
msg.points[5].positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg.points[5].time_from_start = rospy.Time(6.0)
pub.publish(msg)
rospy.sleep(0.5)
def plan(self, joints = None):
""" Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """
if type(joints) is JointState:
self._g.set_joint_value_target(joints.position)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif not joints == None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except:
self.set_joint_value_target(joints)
plan = self._g.get_plan()
plan_msg = RobotTrajectory()
joint_traj = JointTrajectory()
joint_traj.joint_names = plan["joint_trajectory"]["joint_names"]
for point in plan["joint_trajectory"]["points"]:
joint_traj.points.append(JointTrajectoryPoint(
positions = point["positions"],
velocities = point["velocities"],
accelerations = point["accelerations"]))
multi_dof_joint_traj = MultiDOFJointTrajectory()
multi_dof_joint_traj.joint_names = plan["multi_dof_joint_trajectory"]["joint_names"]
multi_dof_joint_traj.frame_ids = plan["multi_dof_joint_trajectory"]["frame_ids"]
multi_dof_joint_traj.child_frame_ids = plan["multi_dof_joint_trajectory"]["child_frame_ids"]
for point in plan["multi_dof_joint_trajectory"]["points"]:
multi_dof_joint_traj_point = MultiDOFJointTrajectoryPoint()
for t in point["transforms"]:
multi_dof_joint_traj_point.poses.append(Transform(
translation = Vector(x = t["translation"]["x"], y = t["translation"]["y"], z = t["translation"]["z"]),
rotation = Quaternion(x = t["rotation"]["x"], y = t["rotation"]["y"],
z = t["rotation"]["z"], w = t["rotation"]["w"])))
multi_dof_joint_traj.points.append(multi_dof_joint_traj_point)
plan_msg.joint_trajectory = joint_traj
plan_msg.multi_dof_joint_trajectory = multi_dof_joint_traj
return plan_msg
def main():
rospy.init_node('send_joints')
pub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
# Create the topic message
traj = JointTrajectory()
traj.header = Header()
# Joint names for UR5
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rate = rospy.Rate(1)
cnt = 0
pts = JointTrajectoryPoint()
traj.header.stamp = rospy.Time.now()
while not rospy.is_shutdown():
cnt += 1
if cnt % 2 == 1:
pts.positions = waypoints[0]
else:
pts.positions = waypoints[1]
pts.time_from_start = rospy.Duration(1.0)
# Set the points to the trajectory
traj.points = []
traj.points.append(pts)
# Publish the message
pub.publish(traj)
rate.sleep()
def jog(vx, vy, vz, ang_x, ang_y, ang_z, action_client):
desired_twist = np.array([vx, vy, vz, ang_x, ang_y, ang_z])
startingPoint = JointTrajectoryPoint()
startingPoint.positions = arm_group.get_current_joint_values()
startingPoint.time_from_start.secs = 0
trajectory = JointTrajectory()
trajectory.points = [startingPoint]
# parameters
N_points = 300
Time_total_sec = 5.0
dt_point2point = Time_total_sec/N_points
time_from_start = 0.0
for i in range(1, N_points):
point = JointTrajectoryPoint()
jacobian = arm_group.get_jacobian_matrix(trajectory.points[i-1].positions)
pseudoInverseJacobian = np.linalg.pinv(jacobian)
jointVelocities = pseudoInverseJacobian.dot(desired_twist)
point.positions = (trajectory.points[i-1].positions + (jointVelocities*dt_point2point)).tolist()
time_from_start = time_from_start + dt_point2point
point.time_from_start = rospy.Duration.from_sec(time_from_start)
trajectory.points.append(point)
trajectory.joint_names = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7']
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
action_client.send_goal(goal)
action_client.wait_for_result()
def talker_2():
try:
pub = rospy.Publisher('/joint_speed_2', JointTrajectory, queue_size=10)
rate = rospy.Rate(2)
trajectory = JointTrajectory()
i_seq = 0
qvel = [0, 0, 0, 0, 0, 0]
while not rospy.is_shutdown():
for i in range(6):
qvel[i] += (np.random.rand() - 0.5) * 2 * 0.01
current_time = rospy.Time.now()
trajectory.header.seq = i_seq
i_seq += 1
trajectory.header.stamp = current_time
trajectory.joint_names = [
'elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
trajectory.points = [JointTrajectoryPoint(velocities=qvel)]
pub.publish(trajectory)
# rospy.loginfo(trajectory)
rate.sleep()
except rospy.ROSInterruptException:
pass
def send_goal(self, position):
# Message type in JointTrajectory
goal_msg = JointTrajectory()
# Joint trajectory points
jtp = JointTrajectoryPoint()
jtp.velocities = [0.0]
jtp.time_from_start.sec = 0
jtp.time_from_start.nanosec = 0
jtp.positions = [position.data]
# Build message
goal_msg.points = [jtp]
goal_msg.joint_names = ["eyes_shift_vertical_joint"]
# Assign goal
goal = FollowJointTrajectory.Goal()
goal.trajectory = goal_msg
self.eye_action_client_vertical.wait_for_server()
self.get_logger().info('Goal: %f' % jtp.positions[0])
self.eye_action_client_vertical.send_goal_async(goal)
def goal_pose_cb(self, data):
rospy.loginfo('Position command: pos={}, speed={}'.format(
data.pos, data.speed))
multiplier = 1000
traj = JointTrajectory()
traj.joint_names = self.joints
jtp = JointTrajectoryPoint()
jtp.positions = list(
rospy.wait_for_message(
u'/whole_body_controller/state',
JointTrajectoryControllerState).actual.positions)
jtp.positions[self.joints.index(
self.gripper_name)] = data.pos / multiplier
jtp.velocities = []
jtp.accelerations = []
jtp.time_from_start = rospy.Duration(0.5)
traj.points = [jtp]
goal = FollowJointTrajectoryGoal()
goal.trajectory = traj
self.follow_joint_traj_client.send_goal_and_wait(goal)
rospy.loginfo('done.')
def main():
rospy.init_node('send_joints')
pub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=100)
# Create the topic message
traj = JointTrajectory()
traj.header = Header()
# Joint names for UR5
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rate = rospy.Rate(10)
pts = JointTrajectoryPoint()
traj.header.stamp = rospy.Time.now()
while not rospy.is_shutdown():
joint_angle_mat = load_file(
"/home/nimblekun/kun_ur5test/src/ur5_test/scripts/kun_5.txt")
cnt = 0
for joint_angle in joint_angle_mat:
pts.positions = joint_angle
pts.time_from_start = rospy.Duration(0.1)
# Set the points to the trajectory
traj.points = []
traj.points.append(pts)
# Publish the message
pub.publish(traj)
cnt += 1
print(cnt)
#tra()
rate.sleep()
def talker():
pub_traj = rospy.Publisher('joint_path_command',
JointTrajectory,
queue_size=10)
rospy.init_node('path_publisher')
traj = JointTrajectory()
traj.header = Header(frame_id='base_link',
stamp=rospy.Time.now() + rospy.Duration(1.0))
traj.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
traj.points = [
JointTrajectoryPoint(positions=[0, 0, 0, 0,
np.deg2rad(30), 0],
velocities=[0, 0, 0, 0, 0, 0],
time_from_start=rospy.Duration(1)),
JointTrajectoryPoint(positions=[0, 0, 0, 0, 0, 0],
velocities=[0, 0, 0, 0, 0, 0])
]
rospy.loginfo(traj)
pub_traj.publish(traj)
rospy.sleep(1.0)
def left_untuck(self):
rospy.loginfo('left_tuck_arm untuck')
# prepare a joint trajectory
msg = JointTrajectory()
msg.joint_names = left_servos
msg.points = list()
#point = JointTrajectoryPoint()
#point.positions = left_to_side
#point.velocities = [ 0.0 for servo in msg.joint_names ]
#point.time_from_start = rospy.Duration(3.0)
#msg.points.append(point)
point = JointTrajectoryPoint()
point.positions = left_forward
point.velocities = [0.0 for servo in msg.joint_names]
point.time_from_start = rospy.Duration(3.0)
msg.points.append(point)
# call action
msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
goal = FollowJointTrajectoryGoal()
goal.trajectory = msg
self._left_client.send_goal(goal)
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 main(data):
pub = rospy.Publisher('/gripper_controller/command',
JointTrajectory,
queue_size=10)
# Create the topic message
arm = JointTrajectory()
arm.header = Header()
# Joint names for UR5
arm.joint_names = ['arm_gripper_right_joint', 'arm_gripper_left_joint']
rate = rospy.Rate(200) # 50hz
rospy.loginfo(arm)
arm.header.stamp = rospy.Time.now()
pts = JointTrajectoryPoint()
pts.positions = [data.position[0], -data.position[0]]
pts.time_from_start = rospy.Duration(1.0)
# Set the points to the trajectory
arm.points = []
arm.points.append(pts)
# Publish the message
pub.publish(arm)
rate.sleep()
def callback(states, joy_move):
cmd_publisher = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
for i in range(len(joy_move.buttons)):
if joy_move.buttons[i] == 1:
break
cmd = JointTrajectory()
cmd.header.stamp = rospy.Time.now()
cmd.joint_names = joint_names
points = []
positions = list(states.position)
point = JointTrajectoryPoint()
factor = 1
if i >= 10 and i < 16:
if i == 12:
factor = -1
positions[i - 10] += joy_move.axes[2] * factor * 0.2
point.positions = positions
point.time_from_start.secs = 0.0
point.time_from_start.nsecs = 157114594
points.append(point)
cmd.points = points
cmd_publisher.publish(cmd)
def send_arm_goal(self, arm_goal):
arm_joint_names = [
'joint_lift', 'joint_waist', 'joint_big_arm', 'joint_forearm',
'joint_wrist_pitching', 'joint_wrist_rotation'
]
rospy.loginfo("Waiting for follow_joint_trajectory server")
self.arm_client.wait_for_server()
rospy.loginfo("Connected to follow_joint_trajectory server")
rospy.sleep(1)
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joint_names
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].time_from_start = rospy.Duration(10)
rospy.loginfo("Preparing for moving the arm to goal position!")
rospy.sleep(1)
arm_goal_pos = FollowJointTrajectoryGoal()
arm_goal_pos.trajectory = arm_trajectory
arm_goal_pos.goal_time_tolerance = rospy.Duration(0)
self.arm_client.send_goal(arm_goal_pos)
rospy.loginfo("Send goal to the trajectory server successfully!")
self.arm_client.wait_for_result()
def setArmJoints(self, joint_states, joint_vel):
arm_joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
arm_joint_positions = joint_states
arm_joint_velocities = joint_vel
trajectory = JointTrajectory()
trajectory.joint_names = arm_joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = arm_joint_positions
trajectory.points[0].velocities = arm_joint_velocities
trajectory.points[0].accelerations = [
0.0 for _ in arm_joint_velocities]
trajectory.points[0].time_from_start = rospy.Duration(0.0001)
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = trajectory
arm_goal.goal_time_tolerance = rospy.Duration(25.0)
rospy.loginfo("Setting Arm positions...")
self.arm_client.send_goal(arm_goal)
# self.arm_client.wait_for_result(rospy.Duration(25.0))
rospy.loginfo("...done")
def pose_cb(self, ps):
""" Perform IK for a goal pose, and send action goal if acheivable"""
rospy.loginfo('made it to pose_cb')
if not self.joint_state_received:
rospy.loginfo('[IKGoalSender] No Joint State Received')
return
req = self.form_ik_request(ps)
ik_goal = self.ik_client(req)
if ik_goal.error_code.val == 1:
traj_point = JointTrajectoryPoint()
traj_point.positions = ik_goal.solution.joint_state.position
traj_point.velocities = ik_goal.solution.joint_state.velocity
traj_point.time_from_start = rospy.Duration(1)
traj = JointTrajectory()
traj.joint_names = self.ik_info.joint_names
traj.points.append(traj_point)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory = traj
#self.traj_client.send_goal(traj_goal)
else:
rospy.loginfo('[IKGoalSender] IK Failed: Cannot reach goal')
self.log_pub.publish(String('IK Failed: Cannot reach goal'))
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
max_duration_scaled = max(dur) / self._speed_scale
seconds = floor(max_duration_scaled)
nanoseconds = (max_duration_scaled - seconds) * 1e9
duration = Duration(seconds=seconds, nanoseconds=nanoseconds)
point.time_from_start = duration.to_msg()
traj.points.append(point)
self._cmd_pub.publish(traj)
def publish_env_arm_cmd(self, position_cmd):
"""Publish environment JointTrajectory msg.
Publish JointTrajectory message to the env_command topic.
Args:
position_cmd (type): Description of parameter `positions`.
Returns:
type: Description of returned object.
"""
if self.safe_to_move:
msg = JointTrajectory()
msg.header = Header()
msg.joint_names = ["elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", \
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]
msg.points = [JointTrajectoryPoint()]
msg.points[0].positions = position_cmd
dur = []
for i in range(len(msg.joint_names)):
# !!! Be careful here with ur_state index
pos = self.ur_state[i]
cmd = position_cmd[i]
max_vel = self.ur_joint_vel_limits[i]
dur.append(
max(abs(cmd - pos) / max_vel, self.min_traj_duration))
msg.points[0].time_from_start = rospy.Duration.from_sec(max(dur))
self.arm_cmd_pub.publish(msg)
rospy.sleep(self.control_period)
return position_cmd
else:
rospy.sleep(self.control_period)
return [0.0] * 6
def shutdown_command(self, signal=0, frame=0):
# Catches Ctrl-c
print('Shutting down....')
self.isInPolishingMode = False
self.update_polishMode()
print('Stop polishing.')
msg_jointVel = JointTrajectory()
msg_jointVel.header.stamp = rospy.Time.now()
msg_jointVel.joint_names = JOINT_NAMES
newPoint = JointTrajectoryPoint()
newPoint.positions = self.joint_pos.tolist()
newPoint.velocities = np.zeros(N_JOINTS).tolist()
for ii in range(3): # repeat several times
msg_jointVel.points.append(newPoint)
self.pub_jointVel.publish(msg_jointVel)
# print('Send zero velocity')
rospy.signal_shutdown('User shutdown.')
print('Thank you master. I hope to see you soon.')
def callback(states, joy_move):
cmd_publisher = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
for i in range(len(list(joy_move.buttons))):
if joy_move.buttons[i] == 1:
break
cmd = JointTrajectory()
cmd.header.stamp = rospy.Time.now()
cmd.joint_names = joint_names
points = []
positions = list(states.position)
point = JointTrajectoryPoint()
factor = 1
j = -1
if i < 6:
if i == 0:
j = 4
elif i == 1:
j = 3
elif i == 2:
j = 5
elif i == 3:
j = 2
elif i == 4:
j = 0
else:
j = 1
temp = list(joy_move.axes)
positions[j] += temp[3] * factor * 0.2
point.positions = positions
point.time_from_start.secs = 0.0
point.time_from_start.nsecs = 157114594
points.append(point)
cmd.points = points
cmd_publisher.publish(cmd)
def move_head(self, pan, tilt):
# Which joints define the head?
head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
# Create a single-point head trajectory with the head_goal as the end-point
head_trajectory = JointTrajectory()
head_trajectory.joint_names = head_joints
head_trajectory.points.append(JointTrajectoryPoint())
head_trajectory.points[0].positions = pan, tilt
head_trajectory.points[0].velocities = [0.0 for i in head_joints]
head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = head_trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
self.head_client.send_goal(head_goal)
# Wait for up to 5 seconds for the motion to complete
self.head_client.wait_for_result(rospy.Duration(2.0))
def make_gripper_posture(self, joint_positions):
# 初始化夹爪的关节运动轨迹
t = JointTrajectory()
# 设置夹爪的关节名称
t.joint_names = ['finger_joint1']
# 初始化关节轨迹点
tp = JointTrajectoryPoint()
# 将输入的关节位置作为一个目标轨迹点
tp.positions = joint_positions
# 设置夹爪的力度
tp.effort = [1.0]
# 设置运动时间
tp.time_from_start = rospy.Duration(1.0)
# 将目标轨迹点加入到运动轨迹中
t.points.append(tp)
# 返回夹爪的关节运动轨迹
return t
def create_goal_for(self, joint_name, new_value):
print "Creating a goal for... " + str(joint_name)
jt = JointTrajectory()
if "head" in joint_name:
jt.joint_names = ['head_1_joint', 'head_2_joint']
elif "torso" in joint_name:
jt.joint_names = ['torso_lift_joint']
elif "arm" in joint_name:
if self.controller_mode == 'position':
jt.joint_names = [
'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint',
'arm_5_joint', 'arm_6_joint', 'arm_7_joint'
]
elif self.controller_mode == 'gravity':
jt.joint_names = ['arm_5_joint', 'arm_6_joint', 'arm_7_joint']
elif "hand" in joint_name:
jt.joint_names = [
'hand_thumb_joint', 'hand_index_joint', 'hand_mrl_joint'
]
elif "gripper" in joint_name:
jt.joint_names = [
'gripper_left_finger_joint', 'gripper_right_finger_joint'
]
jtp = JointTrajectoryPoint()
for j_name in jt.joint_names:
if j_name == joint_name:
jtp.positions.append(new_value)
else:
jtp.positions.append(self.get_joint_position(j_name))
# TODO: maybe tune time for joint groups too, fingers may be too slow
# Goals will take 1.0 seconds + a part relative on how much it should
# move
time_for_goal = 1.0 + \
abs(self.get_joint_position(joint_name) - new_value) * 3.0
jtp.time_from_start = rospy.Duration(time_for_goal)
jt.points.append(jtp)
return jt
# been received on the /clock topic
timeZero = rospy.Time.from_sec(0.0)
while rospy.get_rostime() == timeZero:
# Sleep for a small time to make sure publishing and subscribing works.
rospy.sleep(rospy.Duration(1))
# Add additional sleep because ROS is dumb... (wasn't publishing)
rospy.sleep(rospy.Duration(5))
# Create a JointTrajectory object
traj = JointTrajectory()
# traj.header.stamp = rospy.get_time()
traj.header.frame_id = 'base_link'
# Add the joint names (in any order, although order corresponds to positions,
# velocity, and effort indicie in the JointTrajectoryPoint. Whatever the
# order, the simulator will control the correct joint based on name.)
traj.joint_names = [
'joint_b', 'joint_s', 'joint_t', 'joint_r', 'joint_u', 'joint_l'
]
angle_range = 0.2
time_range = 10
# create a list of JointTrajectoryPoint objects
points = []
N = 100 # number of points
for i in xrange(N):
point = JointTrajectoryPoint()
positions = []
velocities = []
time_from_start = rospy.Duration.from_sec(time_range * (i + 1) /
float(N))
point.time_from_start = time_from_start
position = angle_range * math.sin(2 * math.pi * (i + 1) / float(N))
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()