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 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 __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 execute_JointTrajectory(self, joint_trajectory, wait=True, ):
"""Executes a trajectory. It does not check if the trajectory is safe, nor it performs
any interpolation or smoothing! For a more fine grained control use execute_trajectory.
Parameters:
joint_trajectory: a JointTrajectory msg
wait: block until trajectory is completed
"""
isinstance(joint_trajectory, JointTrajectory)
if joint_trajectory.joint_names[0][0] == "l":
client = self.l_arm_client
self.l_arm_done = False
done_cb=self.__l_arm_done_cb
else:
client = self.r_arm_client
self.r_arm_done = False
done_cb=self.__r_arm_done_cb
goal = JointTrajectoryGoal()
goal.trajectory = joint_trajectory
client.send_goal(goal, done_cb=done_cb)
if wait:
client.wait_for_result()
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 set_arm_state(
self,
jvals,
arm,
wait=False,
):
""" Sets goal for indicated arm (r_arm/l_arm) using provided joint values"""
# Build trajectory message
if len(jvals) == 0:
rospy.logwarn("No %s_arm goal given" % arm[0])
if arm[0] == "l":
self.l_arm_done = True
elif arm[0] == "r":
self.r_arm_done = True
return
# rospy.loginfo("Senging %s_joints: %s" %(arm[0], str(jvals)))
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]
]
if arm[0] == "l":
client = self.l_arm_client
self.l_arm_done = False
elif arm[0] == "r":
client = self.r_arm_client
self.r_arm_done = False
command.points.append(
JointTrajectoryPoint(positions=jvals,
velocities=[0.0] * (len(command.joint_names)),
accelerations=[],
time_from_start=rospy.Duration(
self.time_to_reach)))
#command.header.stamp = rospy.Time.now()
goal = JointTrajectoryGoal()
goal.trajectory = command
# rospy.loginfo("Sending command to %s" % arm)
if arm[0] == "l":
client.send_goal(goal, done_cb=self.__l_arm_done_cb)
elif arm[0] == "r":
client.send_goal(goal, done_cb=self.__r_arm_done_cb)
if wait:
client.wait_for_result()
def start_trajectory(self, trajectory, set_time_stamp=True, wait=True):
"""Creates an action from the trajectory and sends it to the server"""
goal = JointTrajectoryGoal()
goal.trajectory = trajectory
if set_time_stamp:
goal.trajectory.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
if wait:
self.wait()
def send_traj_point(self, point, time=None):
if time is None:
point.time_from_start = rospy.Duration(max(20 * self.dist, 4))
else:
point.time_from_start = rospy.Duration(time)
joint_traj = JointTrajectory()
joint_traj.header.stamp = rospy.Time.now()
joint_traj.header.frame_id = '/torso_lift_link'
joint_traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
joint_traj.points.append(point)
joint_traj.points[0].velocities = [0, 0, 0, 0, 0, 0, 0]
arm_goal = JointTrajectoryGoal()
arm_goal.trajectory = joint_traj
self.arm_traj_client.send_goal(arm_goal)
def send_traj_point(self, point, time=None):
if time is None:
point.time_from_start = rospy.Duration(max(20*self.dist, 4))
else:
point.time_from_start = rospy.Duration(time)
joint_traj = JointTrajectory()
joint_traj.header.stamp = rospy.Time.now()
joint_traj.header.frame_id = '/torso_lift_link'
joint_traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
joint_traj.points.append(point)
joint_traj.points[0].velocities = [0,0,0,0,0,0,0]
arm_goal = JointTrajectoryGoal()
arm_goal.trajectory = joint_traj
self.arm_traj_client.send_goal(arm_goal)
def move( self, trajectory ):
# push trajectory goal to ActionServer
goal = JointTrajectoryGoal()
goal.trajectory = trajectory
goal.trajectory.header.stamp = rospy.Time.now()
self.client.send_goal( goal )
self.client.wait_for_result()
if self.client.get_state == GoalStatus.SUCCEEDED:
rospy.logerr( 'failed to move arm to goal:\n %s'%goal )
return False
else:
return True
def move(self, trajectory):
# push trajectory goal to ActionServer
goal = JointTrajectoryGoal()
goal.trajectory = trajectory
goal.trajectory.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
self.client.wait_for_result()
if self.client.get_state == GoalStatus.SUCCEEDED:
rospy.logerr('failed to move arm to goal:\n %s' % goal)
return False
else:
return True
def set_arm_state(self, jvals, arm, wait = False, ):
""" Sets goal for indicated arm (r_arm/l_arm) using provided joint values"""
# Build trajectory message
if len(jvals) == 0:
rospy.logwarn("No %s_arm goal given" % arm[0])
if arm[0] == "l":
self.l_arm_done = True
elif arm[0] == "r":
self.r_arm_done = True
return
# rospy.loginfo("Senging %s_joints: %s" %(arm[0], str(jvals)))
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]]
if arm[0] == "l":
client = self.l_arm_client
self.l_arm_done = False
elif arm[0] == "r":
client = self.r_arm_client
self.r_arm_done = False
command.points.append(JointTrajectoryPoint(
positions=jvals,
velocities = [0.0] * (len(command.joint_names)),
accelerations = [],
time_from_start = rospy.Duration(self.time_to_reach)))
#command.header.stamp = rospy.Time.now()
goal = JointTrajectoryGoal()
goal.trajectory = command
# rospy.loginfo("Sending command to %s" % arm)
if arm[0] == "l":
client.send_goal(goal, done_cb=self.__l_arm_done_cb)
elif arm[0] == "r":
client.send_goal(goal, done_cb=self.__r_arm_done_cb)
if wait:
client.wait_for_result()
def execute_trajectory(self, trajectory, times, vels, arm, wait=False):
'''
Executes a trajectory. It does not check if the trajectory is safe, nor it performs
any interpolation or smoothing! times and vels can be obtained from ik_utils with the method
@trajectory_times_and_vels
@param trajectory: a list of lists of joints
@param times: a list a times for each element in the trajectory.
@param vels: a list of velocities for each element in the trajectory
@param arm: either "left" or "right"
@param wait: if this call should block until the trajectory is over
'''
command = JointTrajectory()
command.header.stamp = rospy.get_rostime() + rospy.Duration(0.05)
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]]
if arm[0] == "l":
client = self.l_arm_client
self.l_arm_done = False
elif arm[0] == "r":
client = self.r_arm_client
self.r_arm_done = False
for jvals,t,v in zip(trajectory, times, vels):
command.points.append(JointTrajectoryPoint(
positions = jvals,
velocities = v,
accelerations = []*7,
time_from_start = rospy.Duration(t)))
command.header.stamp = rospy.Time.now()
goal = JointTrajectoryGoal()
goal.trajectory = command
if arm[0] == "l":
client.send_goal(goal, done_cb=self.__l_arm_done_cb)
elif arm[0] == "r":
client.send_goal(goal, done_cb=self.__r_arm_done_cb)
if wait:
client.wait_for_result()
def execute_trajectory(self, trajectory, times, vels, arm, wait=False):
'''
Executes a trajectory. It does not check if the trajectory is safe, nor it performs
any interpolation or smoothing! times and vels can be obtained from ik_utils with the method
@trajectory_times_and_vels
@param trajectory: a list of lists of joints
@param times: a list a times for each element in the trajectory.
@param vels: a list of velocities for each element in the trajectory
@param arm: either "left" or "right"
@param wait: if this call should block until the trajectory is over
'''
command = JointTrajectory()
command.header.stamp = rospy.get_rostime() + rospy.Duration(0.05)
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]
]
if arm[0] == "l":
client = self.l_arm_client
self.l_arm_done = False
elif arm[0] == "r":
client = self.r_arm_client
self.r_arm_done = False
for jvals, t, v in zip(trajectory, times, vels):
command.points.append(
JointTrajectoryPoint(positions=jvals,
velocities=v,
accelerations=[] * 7,
time_from_start=rospy.Duration(t)))
command.header.stamp = rospy.Time.now()
goal = JointTrajectoryGoal()
goal.trajectory = command
if arm[0] == "l":
client.send_goal(goal, done_cb=self.__l_arm_done_cb)
elif arm[0] == "r":
client.send_goal(goal, done_cb=self.__r_arm_done_cb)
if wait:
client.wait_for_result()
def send_traj_point(self, point):
point.time_from_start = rospy.Duration(max(20 * self.dist, 4))
#point.time_from_start += rospy.Duration(2.5*abs(self.joint_state_act.positions[4]-point.positions[4])+ 2.5*abs(self.joint_state_act.positions[5]-point.positions[5]) + 2*abs(self.joint_state_act.positions[6]-point.positions[6]))
joint_traj = JointTrajectory()
joint_traj.header.stamp = rospy.Time.now()
joint_traj.header.frame_id = '/torso_lift_link'
joint_traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
joint_traj.points.append(point)
joint_traj.points[0].velocities = [0, 0, 0, 0, 0, 0, 0]
print joint_traj
rarm_goal = JointTrajectoryGoal()
rarm_goal.trajectory = joint_traj
self.r_arm_traj_client.send_goal(rarm_goal)
def send_traj_point(self, point):
point.time_from_start = rospy.Duration(max(20*self.dist, 4))
#point.time_from_start += rospy.Duration(2.5*abs(self.joint_state_act.positions[4]-point.positions[4])+ 2.5*abs(self.joint_state_act.positions[5]-point.positions[5]) + 2*abs(self.joint_state_act.positions[6]-point.positions[6]))
joint_traj = JointTrajectory()
joint_traj.header.stamp = rospy.Time.now()
joint_traj.header.frame_id = '/torso_lift_link'
joint_traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
joint_traj.points.append(point)
joint_traj.points[0].velocities = [0,0,0,0,0,0,0]
print joint_traj
rarm_goal = JointTrajectoryGoal()
rarm_goal.trajectory = joint_traj
self.r_arm_traj_client.send_goal(rarm_goal)
def set_jep(self, arm, q, duration=0.15):
if q is None or len(q) != 7:
raise RuntimeError("set_jep value is " + str(q))
self.arm_state_lock[arm].acquire()
jtg = JointTrajectoryGoal()
jtg.trajectory.joint_names = self.joint_names_list[arm]
jtp = JointTrajectoryPoint()
jtp.positions = q
#jtp.velocities = [0 for i in range(len(q))]
#jtp.accelerations = [0 for i in range(len(q))]
jtp.time_from_start = rospy.Duration(duration)
jtg.trajectory.points.append(jtp)
self.joint_action_client[arm].send_goal(jtg)
self.jep[arm] = q
cep, r = self.arms.FK_all(arm, q)
self.arm_state_lock[arm].release()
o = np.matrix([0.,0.,0.,1.]).T
cep_marker = hv.single_marker(cep, o, 'sphere',
'/torso_lift_link', color=(0., 0., 1., 1.),
scale = (0.02, 0.02, 0.02),
m_id = self.cep_marker_id)
cep_marker.header.stamp = rospy.Time.now()
self.marker_pub.publish(cep_marker)
def test_joint_angles(self):
# for < kinetic
if os.environ['ROS_DISTRO'] >= 'kinetic' :
return True
from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
self.impl_test_joint_angles(larm, JointTrajectoryGoal())
def create_JTG(self,
arm,
pos_arr,
dur_arr,
stamp=None,
vel_arr=None,
acc_arr=None):
# Hack
vel_arr = [[0.] * 7] * len(pos_arr)
acc_arr = [[0.] * 7] * len(pos_arr)
##
# Compute joint velocities and acclereations.
def get_vel_acc(q_arr, d_arr):
vel_arr = [[0.] * 7]
acc_arr = [[0.] * 7]
for i in range(1, len(q_arr)):
vel, acc = [], []
for j in range(7):
vel += [(q_arr[i][j] - q_arr[i - 1][j]) / d_arr[i]]
acc += [(vel[j] - vel_arr[i - 1][j]) / d_arr[i]]
vel_arr += [vel]
acc_arr += [acc]
print vel, acc
return vel_arr, acc_arr
if arm != 1:
arm = 0
jtg = JointTrajectoryGoal()
if stamp is None:
stamp = rospy.Time.now()
else:
jtg.trajectory.header.stamp = stamp
if len(pos_arr) > 1 and (vel_arr is None or acc_arr is None):
v_arr, a_arr = get_vel_acc(pos_arr, dur_arr)
if vel_arr is None:
vel_arr = v_arr
if acc_arr is None:
acc_arr = a_arr
jtg.trajectory.joint_names = self.joint_names_list[arm]
for i in range(len(pos_arr)):
if pos_arr[i] is None or type(pos_arr[i]) is types.NoneType:
continue
jtp = JointTrajectoryPoint()
jtp.positions = pos_arr[i]
if vel_arr is None:
vel = [0.] * 7
else:
vel = vel_arr[i]
jtp.velocities = vel
if acc_arr is None:
acc = [0.] * 7
else:
acc = acc_arr[i]
jtp.accelerations = acc
jtp.time_from_start = rospy.Duration(dur_arr[i])
jtg.trajectory.points.append(jtp)
return jtg
def move(self, angles, time, blocking): goal = JointTrajectoryGoal() goal.trajectory.joint_names = ['head_pan_joint', 'head_tilt_joint'] point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(time) goal.trajectory.points.append(point) self.jta.send_goal(goal)
def _execute_two_arm_trajectory(self, trajectory):
'''
Executes the given trajectory, switching controllers first if needed.
**Args:**
**trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute.
'''
self._controller_manager.switch_controllers(start_controllers=[self._joint_controller])
goal = JointTrajectoryGoal()
goal.trajectory = trajectory
jt_res = self._call_action(self._joint_trajectory_client, goal)
# should actually check this
self._current_handle._set_reached_goal(True)
return jt_res
def command_head(self, angles, time, blocking):
goal = JointTrajectoryGoal()
goal.trajectory.joint_names = ['head_pan_joint', 'head_tilt_joint']
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(time)
goal.trajectory.points.append(point)
return self.send_command('head', goal, blocking, timeout=time)
def init_action_clients(self):
self._aclient_larm = actionlib.SimpleActionClient(
'/larm_controller/joint_trajectory_action', JointTrajectoryAction)
self._aclient_rarm = actionlib.SimpleActionClient(
'/rarm_controller/joint_trajectory_action', JointTrajectoryAction)
self._aclient_head = actionlib.SimpleActionClient(
'/head_controller/joint_trajectory_action', JointTrajectoryAction)
self._aclient_torso = actionlib.SimpleActionClient(
'/torso_controller/joint_trajectory_action', JointTrajectoryAction)
self._aclient_larm.wait_for_server()
rospy.loginfo('ros_client; 1')
self._goal_larm = JointTrajectoryGoal()
rospy.loginfo('ros_client; 2')
self._goal_larm.trajectory.joint_names.append("LARM_JOINT0")
self._goal_larm.trajectory.joint_names.append("LARM_JOINT1")
self._goal_larm.trajectory.joint_names.append("LARM_JOINT2")
self._goal_larm.trajectory.joint_names.append("LARM_JOINT3")
self._goal_larm.trajectory.joint_names.append("LARM_JOINT4")
self._goal_larm.trajectory.joint_names.append("LARM_JOINT5")
rospy.loginfo('ros_client; 3')
self._aclient_rarm.wait_for_server()
self._goal_rarm = JointTrajectoryGoal()
self._goal_rarm.trajectory.joint_names.append("RARM_JOINT0")
self._goal_rarm.trajectory.joint_names.append("RARM_JOINT1")
self._goal_rarm.trajectory.joint_names.append("RARM_JOINT2")
self._goal_rarm.trajectory.joint_names.append("RARM_JOINT3")
self._goal_rarm.trajectory.joint_names.append("RARM_JOINT4")
self._goal_rarm.trajectory.joint_names.append("RARM_JOINT5")
self._aclient_head.wait_for_server()
self._goal_head = JointTrajectoryGoal()
self._goal_head.trajectory.joint_names.append('HEAD_JOINT0')
self._goal_head.trajectory.joint_names.append('HEAD_JOINT1')
self._aclient_torso.wait_for_server()
self._goal_torso = JointTrajectoryGoal()
self._goal_torso.trajectory.joint_names.append('CHEST_JOINT0')
rospy.loginfo(
'Joint names; ' +
'Torso: {}\n\tHead: {}\n\tL-Arm: {}\n\tR-Arm: {}'.format(
self._goal_torso.trajectory.joint_names, self._goal_head.
trajectory.joint_names, self._goal_larm.trajectory.joint_names,
self._goal_rarm.trajectory.joint_names))
def goal_RArm(self):
goal = JointTrajectoryGoal()
goal.trajectory.joint_names.append("RARM_JOINT0")
goal.trajectory.joint_names.append("RARM_JOINT1")
goal.trajectory.joint_names.append("RARM_JOINT2")
goal.trajectory.joint_names.append("RARM_JOINT3")
goal.trajectory.joint_names.append("RARM_JOINT4")
goal.trajectory.joint_names.append("RARM_JOINT5")
return goal
def track_trajectory( pos, vel, acc, time,
arm = 'r', client = None,
stamp = None):
if (acc == None):
acc = np.zeros( pos.shape )
if ( len( pos ) != len( vel ) or len( pos ) != len( time ) ):
rospy.logerr( '[track_trajectory] dimensions do not agree' )
rospy.loginfo( 'arm \'%s\' tracking trajectory with %d points'
%( arm, len( pos ) ) )
if (client == None):
client = init_jt_client(arm)
else:
arm = client.action_client.ns[0:1]; # ignore arm argument
joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) )
# create trajectory message
jt = JointTrajectory()
jt.joint_names = joint_names
jt.points = []
t = 0
for i in range( len( pos ) ):
jp = JointTrajectoryPoint()
jp.time_from_start = rospy.Time.from_sec( time[i] )
jp.positions = pos[i,:]
jp.velocities = vel[i,:]
jp.accelerations = acc[i,:]
jt.points.append( jp )
t += 1
if ( stamp == None ):
stamp = rospy.Time.now()
# push trajectory goal to ActionServer
jt_goal = JointTrajectoryGoal()
jt_goal.trajectory = jt
jt_goal.trajectory.header.stamp = stamp;
client.send_goal(jt_goal)
def command_joint(self, jointangles):
self.check_controllers_ok('joint')
goal = JointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.joint_names = self.joint_names
point = JointTrajectoryPoint(jointangles, [0] * 7, [0] * 7,
rospy.Duration(5.0))
goal.trajectory.points = [
point,
]
self.joint_action_client.send_goal(goal)
def set_ep(self, jep, duration, delay=0.0):
if jep is None or len(jep) != 7:
raise RuntimeError("set_ep value is " + str(jep))
jtg = JointTrajectoryGoal()
jtg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(delay)
jtg.trajectory.joint_names = self.joint_names_list
jtp = JointTrajectoryPoint()
jtp.positions = list(jep)
jtp.time_from_start = rospy.Duration(duration)
jtg.trajectory.points.append(jtp)
self.joint_action_client.send_goal(jtg)
def command_head(self, angles, timeout, blocking=True):
goal = JointTrajectoryGoal()
goal.trajectory.joint_names = ['head_pan_joint', 'head_tilt_joint']
point = JointTrajectoryPoint()
point.positions = angles
point.time_from_start = rospy.Duration(timeout)
goal.trajectory.points.append(point)
if blocking:
self.clients['head'].send_goal_and_wait(goal)
else:
self.clients['head'].send_goal(goal)
return None
def command_joint_angles(self, q, duration=1.0, delay=0.0):
if q is None or len(q) != 7:
return False
jtg = JointTrajectoryGoal()
jtg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(delay)
jtg.trajectory.joint_names = self.joint_names_list
jtp = JointTrajectoryPoint()
jtp.positions = list(q)
jtp.time_from_start = rospy.Duration(duration)
jtg.trajectory.points.append(jtp)
self.joint_action_client.send_goal(jtg)
return True
def execute_joint_traj(self, joint_trajectory, timing):
'''Moves the arm through the joint sequence.
Args:
joint_trajectory (ArmState[]): List of ArmStates to move to
(either for the right or left arm).
timing (duration[])
'''
# First, do filtering on the trajectory to fix the velocities.
trajectory = JointTrajectory()
# Initialize the server.
# When to start the trajectory: TRAJECTORY_START_DELAY seconds from now
trajectory.header.stamp = rospy.Time.now() + TRAJECTORY_START_DELAY
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 = [TRAJ_VELOCITY] * 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=TRAJECTORY_ALLOWED_TIME)
rospy.loginfo('\tTrajectory for ' + self._side() +
' arm has been filtered.')
# TODO(mcakmak): Check output.error_code.
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory = output.trajectory
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
TRAJECTORY_START_DELAY)
traj_goal.trajectory.joint_names = self.joint_names
# Sends the goal to the trajectory server
self.traj_action_client.send_goal(traj_goal)
def set_jointangles(self, arm, q, duration=0.15):
rospy.logwarn('Currently ignoring the arm parameter.')
# for i,p in enumerate(self.r_arm_pub_l):
# p.publish(q[i])
jtg = JointTrajectoryGoal()
jtg.trajectory.joint_names = self.joint_names_list
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.send_goal(jtg)
def pose_(position, joints, client):
goal = JointTrajectoryGoal()
goal.trajectory.joint_names = joints
goal.trajectory.points = [ JointTrajectoryPoint() ]
goal.trajectory.points[0].velocities = [0.0] * len(joints);
goal.trajectory.points[0].positions = [0.0] * len(joints);
for i, j in enumerate(joints):
goal.trajectory.points[0].positions[i] = position[j]
goal.trajectory.points[0].time_from_start = rospy.Duration.from_sec(1.0)
client.send_goal(goal)
def pose_cb(self, ps):
""" Perform IK for a goal pose, and send action goal if acheivable"""
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 command_joint_trajectory(self, arm, angles, times, blocking):
print angles
timeout = times[-1] + 1.0
goal = JointTrajectoryGoal()
goal.trajectory.joint_names = self.get_joint_names(arm)
for (ang, t) in zip(angles, times):
point = JointTrajectoryPoint()
point.positions = ang
point.time_from_start = rospy.Duration(t)
goal.trajectory.points.append(point)
goal.trajectory.header.stamp = rospy.Time.now()
return self.send_command("%s_joint" % arm, goal, blocking, timeout)
def move_to_joints(self, joints, time_to_joint):
'''Moves the arm to the desired joints'''
# Setup the goal
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.joint_names = self.joint_names
velocities = [0] * len(joints)
traj_goal.trajectory.points.append(JointTrajectoryPoint(
positions=joints,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
self.traj_action_client.send_goal(traj_goal)
def pose_cb(self, ps):
""" Perform IK for a goal pose, and send action goal if acheivable"""
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 move_arm(self, angles): #input: angles
goal = JointTrajectoryGoal()
char = self.arm_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()
point.positions = angles
point.time_from_start = rospy.Duration(3)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def move(self):
goal = JointTrajectoryGoal()
goal.trajectory.joint_names = self.joint_names
point = JointTrajectoryPoint()
angles = self.angles
for i in xrange(6 * 4):
angles[6] += i * 0.1
point.positions = angles
point.time_from_start = rospy.Duration(1 * 0.25 * i)
rospy.loginfo(str(angles[6]) + ": " + str(rospy.Duration(1 * 0.25 * i)))
goal.trajectory.points.append(deepcopy(point))
self.jta.send_goal_and_wait(goal)
def move_body_part(service, joints, positions):
global arm_move_duration
client = actionlib.SimpleActionClient(service, JointTrajectoryAction)
client.wait_for_server()
goal = JointTrajectoryGoal()
goal.trajectory.joint_names = joints
point = get_trajectory_point(positions,
roslib.rostime.Duration(arm_move_duration))
goal.trajectory.points.append(point)
goal.trajectory.header.stamp = rospy.Time.now()
client.send_goal(goal, done_cb=task_completed)
#print "Goal Sent"
client.wait_for_result()
def main():
joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
N = len( joint_names )
# lower_limit = asarray( [-2.13, -0.35, -3.75, -2.32, -2.0 * pi, -2.09, -2.0 * pi ] )
# upper_limit = asarray( [ 0.56,
# 0.8, #1.29,
# 0.65, 0.0, 2.0 * pi, 0.0, 2.0 * pi ] )
lower_limit = asarray( [2.13,
-0.35,
-0.5, #3.75,
-2.0, #-2.32,
-2.0 * pi,
-2.09,
-2.0 * pi ] )
upper_limit = asarray( [-0.56,
0.6, #1.29,
2, #0.65,
0.0,
2.0 * pi,
0.0,
2.0 * pi ] )
A = (lower_limit - upper_limit) * 1/4
f = 0.1
f1 = asarray([f * sqrt(2),
f * sqrt(2),
f * sqrt(2),
f * sqrt(9),
f * sqrt(7),
f * sqrt(1),
f * sqrt(7)])
f2 = asarray([0.4,
0.4,
0.6,
0.9,
1.1,
0.3,
0.9])
start_time = 0.0
dt = 0.001
max_time = 60.0
t = linspace( 0.0, max_time - dt, int( max_time / dt ) )
q = zeros( (N, len( t )))
dq = zeros( (N, len( t )))
ddq = zeros( (N, len( t )))
# start_pos = [ 0, 0, 0, 0, 0, 0, 0 ]
# joint = 2;
for i in range(N):
#if i == joint:
q[i,:] = A[i] * sin( 2.0 * pi * f1[i] * t ) \
+ A[i]/3.0 * sin( 2.0 * pi * f2[i] * t ) + ( upper_limit[i] + lower_limit[i]) / 2.0
dq[i,:] = 2.0 * pi * f1[i] * A[i] * cos( 2.0 * pi * f1[i] * t ) \
+ 2.0/3.0 * pi * f2[i] * A[i] * cos( 2.0 * pi * f2[i] * t )
ddq[i,:] = - 4.0 * pi ** 2 * f1[i] ** 2 * A[i] * sin( 2.0 * pi * f1[i] * t ) \
- 4.0/3.0 * pi ** 2 * f2[i] ** 2 * A[i] * sin( 2.0 * pi * f2[i] * t )
#else:
# q[i,:] = ones( (1, len( t )) ) * start_pos[i]
# plt.subplot( 3, 1, 1);
# plt.plot( q[joint,:] )
# plt.subplot( 3, 1, 2);
# plt.plot( dq[joint,:] )
# plt.subplot( 3, 1, 3);
# plt.plot( ddq[joint,:] )
# plt.show()
rospy.init_node( 'gpr_controller_trajectory_generator' )
client = actionlib.SimpleActionClient( 'l_arm_controller/joint_trajectory_action',
JointTrajectoryAction )
client.wait_for_server()
jt = JointTrajectory()
jt.joint_names = joint_names
jt.points = []
# start at neutral position
jp = JointTrajectoryPoint()
jp.time_from_start = rospy.Time.from_seconds( start_time )
jp.positions = q[:,0]
jp.velocities = [0] * len( joint_names )
jp.accelerations = [0] * len( joint_names )
jt.points.append( jp )
# add precomputed trajectory
for i in range( len( t ) ):
jp = JointTrajectoryPoint()
jp.time_from_start = rospy.Time.from_seconds( t[i] + start_time )
jp.positions = q[:,i]
jp.velocities = dq[:,i]
jp.accelerations = ddq[:,i]
jt.points.append( jp )
# push trajectory goal to ActionServer
goal = JointTrajectoryGoal()
goal.trajectory = jt
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(4.0)
client.send_goal(goal)
client.wait_for_result()
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()
