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 __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 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 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 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 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 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 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 _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 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 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 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 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 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 executeTraj(trajdata):
#torsogoal = SingleJointPositionGoal()
torso_goal = JointTrajectoryGoal()
arm_goal = JointTrajectoryGoal()
# parse trajectory data into the ROS structure
tokens = trajdata.split()
numpoints = int(tokens[0])
dof = int(tokens[1])
trajoptions = int(tokens[2])
numvalues = dof
offset = 0
if trajoptions & 4: # timestamps
numvalues += 1
offset += 1
if trajoptions & 8: # base transforms
numvalues += 7
if trajoptions & 16: # velocities
numvalues += dof
if trajoptions & 32: # torques
numvalues += dof
arm_res = MoveToHandPositionResponse()
torso_res = MoveToHandPositionResponse()
for i in range(numpoints):
start = 3+numvalues*i
torso_pt=JointTrajectoryPoint()
arm_pt=JointTrajectoryPoint()
k=0
for j in robot.GetJoints(manip.GetArmIndices()):
pos=float(tokens[start+offset+j.GetDOFIndex()])
if j.GetName()=='torso_lift_joint':
if i>0:
prevpos=torso_res.traj.points[-1].positions[0]
newpos=prevpos+angleDistance(pos,prevpos)
torso_pt.positions.append(newpos)
else:
torso_pt.positions.append(pos)
else:
if i>0:
prevpos=arm_res.traj.points[-1].positions[k]
newpos=prevpos+angleDistance(pos,prevpos)
arm_pt.positions.append(newpos)
else:
arm_pt.positions.append(pos)
k=k+1
if trajoptions & 4:
arm_pt.time_from_start = rospy.Duration(float(tokens[start]))
torso_pt.time_from_start = rospy.Duration(float(tokens[start]))
arm_res.traj.joint_names = \
filter(lambda jn: jn!='torso_lift_joint',
[j.GetName() for j in robot.GetJoints(manip.GetArmIndices())])
arm_res.traj.points.append(arm_pt)
torso_res.traj.joint_names = ['torso_lift_joint']
torso_res.traj.points.append(torso_pt)
arm_goal.trajectory = arm_res.traj
print 'sending arm JointTrajectoryGoal'
arm_joint_action_client.send_goal(arm_goal)
torso_goal.trajectory = torso_res.traj
print 'sending torso JointTrajectoryGoal'
torso_joint_action_client.send_goal(torso_goal)
arm_joint_action_client.wait_for_result()
torso_joint_action_client.wait_for_result()
return arm_goal,torso_goal
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()
rospy.wait_for_service('MoveToHandPosition')
MoveToHandPositionFn = rospy.ServiceProxy(
'MoveToHandPosition', orrosplanning.srv.MoveToHandPosition)
req = orrosplanning.srv.MoveToHandPositionRequest()
req.hand_frame_id = 'r_gripper_palm_link'
#req.hand_goal.pose.position = geometry_msgs.msg.Point(.6,-.189,.8)
req.hand_goal.pose.position = geometry_msgs.msg.Point(0.293, -0.677, 0.963)
req.hand_goal.pose.orientation = geometry_msgs.msg.Quaternion(0, 0, 0, 1)
req.hand_goal.header.frame_id = 'base_footprint'
req.manip_name = 'rightarm'
print 'sending MoveToHandPosition request'
res = MoveToHandPositionFn(req)
#print res
#use_slip_detection = 0
#tf_listener = tf.TransformListener()
#cms = [controller_manager.ControllerManager('r', tf_listener, use_slip_detection),
# controller_manager.ControllerManager('l', tf_listener, use_slip_detection)]
whicharm = 'r'
joint_trajectory_action_name = '' + whicharm + '_arm_controller/joint_trajectory_action'
joint_action_client = actionlib.SimpleActionClient(
joint_trajectory_action_name, JointTrajectoryAction)
print 'waiting for', joint_trajectory_action_name
print 'have you set ROS_IP?'
joint_action_client.wait_for_server()
goal = JointTrajectoryGoal()
goal.trajectory = res.traj
print 'sending JointTrajectoryGoal'
joint_action_client.send_goal(goal)
def executeTraj(trajdata, whicharm='l'):
raw_input('press enter to move the robot')
if manip_name.find('torso') > -1:
torso_goal = JointTrajectoryGoal()
arm_goal = JointTrajectoryGoal()
tokens = trajdata.split()
numpoints = int(tokens[0])
dof = int(tokens[1])
trajoptions = int(tokens[2])
numvalues = dof
offset = 0
if trajoptions & 4: # timestamps
numvalues += 1
offset += 1
if trajoptions & 8: # base transforms
numvalues += 7
if trajoptions & 16: # velocities
numvalues += dof
if trajoptions & 32: # torques
numvalues += dof
arm_res = MoveToHandPositionResponse()
if manip_name.find('torso') > -1:
torso_res = MoveToHandPositionResponse()
for i in range(numpoints):
start = 3 + numvalues * i
torso_pt = JointTrajectoryPoint()
arm_pt = JointTrajectoryPoint()
k = 0
for j in robot.GetJoints(manip.GetArmIndices()):
pos = float(tokens[start + offset + j.GetDOFIndex()])
if j.GetName() == 'torso_lift_joint':
if i > 0:
prevpos = torso_res.traj.points[-1].positions[0]
newpos = prevpos + angleDistance(pos, prevpos)
torso_pt.positions.append(newpos)
else:
torso_pt.positions.append(pos)
else:
if i > 0:
prevpos = arm_res.traj.points[-1].positions[k]
newpos = prevpos + angleDistance(pos, prevpos)
arm_pt.positions.append(newpos)
else:
arm_pt.positions.append(pos)
k = k + 1
if trajoptions & 4:
arm_pt.time_from_start = rospy.Duration(float(tokens[start])) * 2
if manip_name.find('torso') > -1:
torso_pt.time_from_start = rospy.Duration(float(
tokens[start])) * 2
arm_res.traj.joint_names = \
filter(lambda jn: jn!='torso_lift_joint',
[j.GetName() for j in robot.GetJoints(manip.GetArmIndices())])
arm_res.traj.points.append(arm_pt)
if manip_name.find('torso') > -1:
torso_res.traj.joint_names = ['torso_lift_joint']
torso_res.traj.points.append(torso_pt)
arm_goal.trajectory = arm_res.traj
if manip_name.find('torso') > -1:
torso_goal.trajectory = torso_res.traj
print 'sending torso JointTrajectoryGoal'
torso_joint_action_client.send_goal(torso_goal)
print 'sending arm JointTrajectoryGoal'
if whicharm == 'l':
l_arm_joint_action_client.send_goal(arm_goal)
l_arm_joint_action_client.wait_for_result()
else:
r_arm_joint_action_client.send_goal(arm_goal)
r_arm_joint_action_client.wait_for_result()
if manip_name.find('torso') > -1:
torso_joint_action_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()