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_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 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_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 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 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 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 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 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 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_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 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 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 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 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 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(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 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 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 move(self, angles, time, blocking):
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()
point.positions = angles
point.time_from_start = rospy.Duration(time)
goal.trajectory.points.append(point)
if blocking:
self.jta.send_goal_and_wait(goal)
else:
self.jta.send_goal(goal)
def command_arm_trajectory(self,
arm,
angles,
times_from_start,
blocking=True,
logging=False,
time_buffer=5.0):
# angles is a list of joint angles, times is a list of times from start
# When calling joints on an arm, needs to be called with all the angles in the arm
# rospy.Duration is fine with taking floats, so the times can be floats
assert len(angles) == len(times_from_start)
goal = JointTrajectoryGoal()
goal.trajectory.joint_names = get_arm_joint_names(arm)
for positions, time_from_start in zip(angles, times_from_start):
point = JointTrajectoryPoint()
point.positions = positions
# point.velocities = [(ang[i] - last_position[i])/(t - last_time) for i in range(7)]
# point.velocities = [0.0 for _ in range(len(positions))]
point.time_from_start = rospy.Duration(time_from_start)
goal.trajectory.points.append(point)
# goal.trajectory.header.stamp = rospy.Time.now()
# Using rospy.Time.now() is bad because the PR2 might be a few seconds ahead.
# In that case, it clips off the first few points in the trajectory.
# The clipping causes a jerking motion which can ruin the motion.
goal.trajectory.header.stamp = rospy.Time(0)
self.publish_joint_trajectories(goal.trajectory)
if logging:
self.start_joint_logging(get_arm_joint_names(arm))
# TODO(caelan): multiplicative time_buffer
timeout = times_from_start[-1] + time_buffer
result = self._send_command("%s_joint" % arm,
goal,
blocking=blocking,
timeout=timeout)
if logging:
self.stop_joint_logging()
actual = np.array([
self.joint_positions[joint_name]
for joint_name in goal.trajectory.joint_names
])
desired = np.array(goal.trajectory.points[-1].positions)
print('Error:',
zip(goal.trajectory.joint_names, np.round(actual - desired, 5)))
return result
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 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()
