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 scale_trajectory_speed(traj, scale):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the planned trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and scale the time from start, speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajectory
return new_traj
def build_traj(start, end, duration):
if sorted(start.name) <> sorted(end.name):
rospy.logerr('Start and End position joint_names mismatch')
raise
# assume the start-position joint-ordering
joint_names = start.name
# 始点定義
start_pt = JointTrajectoryPoint()
start_pt.positions = start.position
start_pt.velocities = [0]*len(start.position)
start_pt.time_from_start = rospy.Duration(0.0) # 始点なので、待ち時間 = 0 [sec]
# 中間地点定義
middle_pt = JointTrajectoryPoint()
for j in joint_names:
idx = end.name.index(j)
middle_pt.positions.append((start.position[idx] + end.position[idx])/2.0)
middle_pt.velocities.append(0)
middle_pt.time_from_start = rospy.Duration(duration) # 中間地点までの到達時間 = duration [sec]
# 執着地点
end_pt = JointTrajectoryPoint()
for j in joint_names:
idx = end.name.index(j)
end_pt.positions.append(end.position[idx])
end_pt.velocities.append(0.0)
end_pt.time_from_start = rospy.Duration(duration*2) # 終点までの到達時間 = duration*2 [sec]
return JointTrajectory(joint_names=joint_names, points=[start_pt, middle_pt, end_pt])
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 build_traj(start, end, duration):
# print start.name
# print end.name
if sorted(start.name) <> sorted(end.name):
rospy.logerr('Start and End position joint_names mismatch')
raise
# assume the start-position joint-ordering
joint_names = start.name
start_pt = JointTrajectoryPoint()
start_pt.positions = start.position
start_pt.velocities = [0.05]*len(start.position)
start_pt.time_from_start = rospy.Duration(0.0)
end_pt = JointTrajectoryPoint()
# end_pt = start_pt;
# end_pt.positions[3] = end_pt.positions[3]+.1
for j in joint_names:
idx = end.name.index(j)
end_pt.positions.append(end.position[idx]) # reorder to match start-pos joint ordering
end_pt.velocities.append(0)
end_pt.time_from_start = rospy.Duration(duration)
return JointTrajectory(joint_names=joint_names, points=[start_pt, end_pt])
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 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 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 createHeadGoal(j1, j2):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1 and j2 for the joint positions
@arg j1 float value for head_1_joint
@arg j2 float value for head_2_joint
@returns FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('head_1_joint')
fjtg.trajectory.joint_names.append('head_2_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.points.append(point)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def trajectory_cb(self, traj):
rospy.loginfo("Received a new trajectory, beginning the split..")
newtraj_arm = JointTrajectory()
newtraj_arm.header.stamp = traj.header.stamp
newtraj_arm.header.frame_id = traj.header.frame_id
for name in traj.joint_names:
if name[:5] == "Joint":
newtraj_arm.joint_names += (name,)
for point in traj.points:
newpoint = JointTrajectoryPoint()
for i, name in enumerate(traj.joint_names):
if name[:5] == "Joint":
newpoint.positions += (point.positions[i],)
newpoint.velocities += (point.velocities[i],)
newpoint.accelerations += (point.accelerations[i],)
newpoint.time_from_start = point.time_from_start
newtraj_arm.points += (newpoint,)
# rospy.loginfo("Sending trajectory:\n" + str(newtraj_arm))
rospy.loginfo("Publishing the arm trajectory")
self.traj_publisher.publish(newtraj_arm)
def control_loop(self):
if self.commands is None:
raise Exception('Command list is empty. Was the control plan loaded?')
# loop through the command list
for cmd in self.commands:
# wait for proxy to be in active state
self.wait_for_state(self._proxy_state_running)
# process commands
cmd_spec_str = None
spec = None
t = cmd[ProxyCommand.key_command_type]
if not (t == "noop"):
cmd_spec_str = cmd[ProxyCommand.key_command_spec]
if not isinstance(cmd_spec_str, basestring):
spec = float(cmd_spec_str)
else:
spec = self.positions[cmd_spec_str]
rospy.loginfo("Command type: " + t + ", spec: " + str(cmd_spec_str) + ", value: " + str(spec))
# check for any wait depends
self.wait_for_depend(cmd)
# execute command
# could do this with a dictionary-based function lookup, but who cares
if t == 'noop':
rospy.loginfo("Command type: noop")
self.wait_for_depend(cmd)
elif t == 'sleep':
rospy.loginfo("sleep command")
v = float(spec)
rospy.sleep(v)
elif t == 'move_gripper':
rospy.loginfo("gripper command")
self.move_gripper(spec)
elif t == 'move_arm':
rospy.loginfo("move_arm command")
rospy.logdebug(spec)
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = self.arm_joint_names
jtp = JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(0.5) # fudge factor for gazebo controller
jtp.positions = spec
jtp.velocities = [0]*len(spec)
goal.trajectory.points.append(jtp)
self._arm_goal = copy.deepcopy(goal)
self.move_arm()
elif t == 'plan_exec_arm':
rospy.loginfo("plan and execute command not implemented")
raise NotImplementedError()
else:
raise Exception("Invalid command type: " + str(cmd.type))
# check for any set dependencies action
self.set_depend(cmd)
# check for any clear dependencies action
self.clear_depend(cmd)
def 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_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 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 test_trajectory(self):
# our goal variable
goal = FollowJointTrajectoryGoal()
# First, the joint names, which apply to all waypoints
goal.trajectory.joint_names = ('shoulder_pitch_joint',
'shoulder_pan_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_pitch_joint',
'wrist_roll_joint')
# We will have two waypoints in this goal trajectory
# First trajectory point
# To be reached 1 second after starting along the trajectory
point = JointTrajectoryPoint()
point.positions = [0.0] * 7
point.velocities = [1.0] * 7
point.time_from_start = rospy.Duration(3.0)
goal.trajectory.points.append(point)
#we are done; return the goal
return goal
def move(self, angles):
goal = JointTrajectoryGoal()
char = self.name[0] #either 'r' or 'l'
goal.trajectory.joint_names = [char+'_shoulder_pan_joint',
char+'_shoulder_lift_joint',
char+'_upper_arm_roll_joint',
char+'_elbow_flex_joint',
char+'_forearm_roll_joint',
char+'_wrist_flex_joint',
char+'_wrist_roll_joint']
point = JointTrajectoryPoint()
new_angles = list(angles)
#replace angles that have None with last commanded joint position from this Arm class.
for i,ang in enumerate(angles):
if ang is None:
assert self.last_angles is not None
new_angles[i] = self.last_angles[i]
self.last_angles = new_angles
point.positions = new_angles
point.time_from_start = rospy.Duration(3)
goal.trajectory.points.append(point)
self.jta.send_goal_and_wait(goal)
def 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 execute(self, userdata):
rospy.loginfo('In create_move_group_joints_goal')
_move_joint_goal = FollowJointTrajectoryGoal()
_move_joint_goal.trajectory.joint_names = userdata.move_joint_list
jointTrajectoryPoint = JointTrajectoryPoint()
jointTrajectoryPoint.positions = userdata.move_joint_poses
for x in range(0, len(userdata.move_joint_poses)):
jointTrajectoryPoint.velocities.append(0.0)
jointTrajectoryPoint.time_from_start = rospy.Duration(2.0)
_move_joint_goal.trajectory.points.append(jointTrajectoryPoint)
for joint in _move_joint_goal.trajectory.joint_names:
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
_move_joint_goal.goal_tolerance.append(goal_tol)
_move_joint_goal.goal_time_tolerance = rospy.Duration(2.0)
_move_joint_goal.trajectory.header.stamp = rospy.Time.now()
rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal))
userdata.move_joint_goal = _move_joint_goal
return 'succeeded'
def 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 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 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 createHeadGoal(self):
(roll, pitch, yaw) = euler_from_quaternion([self.last_oculus_msg.pose.orientation.x,
self.last_oculus_msg.pose.orientation.y,
self.last_oculus_msg.pose.orientation.z,
self.last_oculus_msg.pose.orientation.w])
rospy.loginfo("roll, pitch, yaw: ")
rospy.loginfo(str(roll) + " " + str(pitch) + " " + str(yaw))
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory.joint_names.append('head_1_joint') # 1 positivo izquierda, -1 derecha
head_goal.trajectory.joint_names.append('head_2_joint') # -1 arriba, 1 abajo
# Current position trajectory point
jtp_current = JointTrajectoryPoint()
jtp_current.positions.append(self.last_head_state.actual.positions[0])
jtp_current.positions.append(self.last_head_state.actual.positions[1])
jtp_current.velocities.append(self.last_head_state.actual.velocities[0])
jtp_current.velocities.append(self.last_head_state.actual.velocities[1])
jtp_current.time_from_start = rospy.Duration(0.0)
# Next goal position
jtp = JointTrajectoryPoint()
jtp.positions.append(yaw *-1)
jtp.positions.append(pitch *-1)
# jtp.positions.append((yaw *-1) + 1.5707963267948966) # + 180 deg
# jtp.positions.append((roll + 1.5707963267948966) *-1) # + 180 deg because of torso2
jtp.velocities.append(0.0)
jtp.velocities.append(0.0)
rospy.loginfo("Sending: " + str(jtp.positions))
#jtp.time_from_start.secs = 1
jtp.time_from_start.nsecs = 100
head_goal.trajectory.points.append(jtp_current)
head_goal.trajectory.points.append(jtp)
head_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
return head_goal
def stage_3(self):
# Move arm to grasping position
rospy.loginfo("Begin stage 3")
# Create a joint state publisher
self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory)
rospy.sleep(1)
# How fast will we update the robot's movement? in Hz
rate = 10
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Define a joint trajectory message for grasp position
rospy.loginfo("Position arm in grasp position")
msg = JointTrajectory()
msg.header.seq = 1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '0'
msg.joint_names.append('joint_1')
msg.joint_names.append('joint_2')
msg.joint_names.append('joint_3')
msg.joint_names.append('joint_4')
msg.joint_names.append('joint_5')
# Define joint trajectory points
point = JointTrajectoryPoint()
point.positions = [0.0, 0.0, 0.0, 1.6, 0.1]
point.velocities = [20.0, 20.0, 20.0, 10.0, 10.0]
point.time_from_start = rospy.Duration(3.0)
msg.points.append(point)
# publish joint stage message
self.arm_pub.publish(msg)
rospy.sleep(3)
def test_joint_trajectory_action(self):
larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
larm.wait_for_server()
try:
self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
except tf.Exception:
self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
(trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
goal = JointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.joint_names.append("J1")
goal.trajectory.joint_names.append("J2")
goal.trajectory.joint_names.append("J3")
goal.trajectory.joint_names.append("J4")
goal.trajectory.joint_names.append("J5")
goal.trajectory.joint_names.append("J6")
goal.trajectory.joint_names.append("J7")
point = JointTrajectoryPoint()
point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
point.time_from_start = rospy.Duration(5.0)
goal.trajectory.points.append(point)
larm.send_goal(goal)
larm.wait_for_result()
(trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
def createHandGoal(side, j1, j2, j3):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
with the hand specified in side
@arg side string 'right' or 'left'
@arg j1 float value for joint 'hand_'+side+'_thumb_joint'
@arg j2 float value for joint 'hand_'+side+'_middle_joint'
@arg j3 float value for joint 'hand_'+side+'_index_joint'
@return FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('hand_'+side+'_thumb_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_middle_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_index_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.positions.append(j3)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
fjtg.trajectory.points.append(point)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def states_to_trajectory(states, time_step=0.1):
"""
Converts a list of RobotState/JointState in Robot Trajectory
:param robot_states: list of RobotState
:param time_step: duration in seconds between two consecutive points
:return: a Robot Trajectory
"""
rt = RobotTrajectory()
for state_idx, state in enumerate(states):
if isinstance(state, RobotState):
state = state.joint_state
jtp = JointTrajectoryPoint()
jtp.positions = state.position
# jtp.velocities = state.velocity
# Probably does not make sense to keep velocities and efforts here
# jtp.effort = state.effort
jtp.time_from_start = rospy.Duration(state_idx*time_step)
rt.joint_trajectory.points.append(jtp)
if len(states) > 0:
if isinstance(states[0], RobotState):
rt.joint_trajectory.joint_names = states[0].joint_state.name
else:
rt.joint_trajectory.joint_names = states[0].joint_names
return rt
def traj_speed_up(traj, spd=3.0):
new_traj = RobotTrajectory()
new_traj = traj
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * spd
point.accelerations[j] = point.accelerations[j] * spd * spd
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
def convert_trajectory(self, traj):
""" Converts a trajectory into a joint trajectory
Args:
traj: Trajectory to convert
Returns:
joint trajectory
"""
new_traj = JointTrajectory()
new_traj.header = traj.header
# Take each joint in the trajectory and add it to the joint trajectory
for joint_name in traj.joint_names:
new_traj.joint_names.append(self.joint_map[joint_name].joint_name)
# For each poiint in the trajectory
for point in traj.points:
new_point = JointTrajectoryPoint()
for i, pos in enumerate(point.positions):
new_point.positions.append(self.joint_map[new_traj.joint_names[i]].convert_angle(pos))
for i, vel in enumerate(point.velocities):
new_point.velocities.append(self.joint_map[new_traj.joint_names[i]].convert_velocity(vel))
new_point.time_from_start = point.time_from_start
new_traj.points.append(new_point)
return new_traj
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
if self._mode == "velocity":
velocities = [0.0] * len(joint_names)
cmd = dict(zip(joint_names, velocities))
while not self._server.is_new_goal_available() and self._alive:
self._limb.set_joint_velocities(cmd)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
elif self._mode == "position" or self._mode == "position_w_id":
raw_pos_mode = self._mode == "position_w_id"
if raw_pos_mode:
pnt = JointTrajectoryPoint()
pnt.positions = self._get_current_position(joint_names)
if dimensions_dict["velocities"]:
pnt.velocities = [0.0] * len(joint_names)
if dimensions_dict["accelerations"]:
pnt.accelerations = [0.0] * len(joint_names)
while not self._server.is_new_goal_available() and self._alive:
self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
# zero inverse dynamics feedforward command
if self._mode == "position_w_id":
pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
self._pub_ff_cmd.publish(ff_pnt)
if self._cuff_state:
self._limb.exit_control_mode()
break
rospy.sleep(1.0 / self._control_rate)
def point():
global param_change_flag
global transition
global ang1v, ang2v, vel_1v, vel_2v
rospy.init_node('point_control')
pub = rospy.Publisher('/arm_twist', JointTrajectoryPoint, queue_size=10)
pub_fp = rospy.Publisher('/arm_fp', Float64, queue_size=10)
pub_pv1 = rospy.Publisher('/pv1', Float64, queue_size=10)
pub_pn1 = rospy.Publisher('/pn1', Float64, queue_size=10)
pub_mix1 = rospy.Publisher('/mix1', Float64, queue_size=10)
rospy.Subscriber('/arm_start_stop', Bool, run_cb)
rospy.Subscriber("/offset_joint_azim", UInt16, offset_joint_azim_cb)
rospy.Subscriber("/offset_joint_elev", UInt16, offset_joint_elev_cb)
rospy.Subscriber("/vel_1", Float64, vel_1_cb)
rospy.Subscriber("/vel_2", Float64, vel_2_cb)
grados_joint_azim = rospy.get_param("/grados_joint_azim", 360)
rospy.loginfo("%s is %s", rospy.resolve_name('grados_joint_azim'), grados_joint_azim)
grados_joint_elev = rospy.get_param("/grados_joint_elev", 45)
rospy.loginfo("%s is %s", rospy.resolve_name("/grados_joint_elev"),grados_joint_elev)
ang1 = grados_joint_azim*np.pi/180 #np.pi*2
ang2 = grados_joint_elev*np.pi/180 #np.pi*3/4
offset_joint_azim = rospy.get_param("/offset_joint_azim", 0)
rospy.loginfo("%s is %s", rospy.resolve_name("/offset_joint_azim"), offset_joint_azim)
offset_joint_elev = rospy.get_param("/offset_joint_elev", 0)
rospy.loginfo("%s is %s", rospy.resolve_name("/offset_joint_elev"), offset_joint_elev)
ang1_offset = offset_joint_azim*np.pi/180 #np.pi*2
ang2_offset = offset_joint_elev*np.pi/180 #np.pi*3/4
pub_period = 0.05
#vel_1 = rospy.get_param("/vel_1")
#rospy.loginfo("%s is %s", rospy.resolve_name('/vel_1'),vel_1)
#vel_2 = rospy.get_param("/vel_2")
#rospy.loginfo("%s is %s", rospy.resolve_name('/vel_2'),vel_2)
start_time = time.time()
time.sleep(0.01)
next_point_time = 0
dt = 0
tau = 1.0
while not rospy.is_shutdown():
if(time.time() > next_point_time) and run_var==True:
dt = time.time() - start_time
if param_change_flag == True:
param_change_flag = False
transition = True
dtv = dt
if transition == True:
fp = ((6/tau**5)*((dt-dtv)**5)) -((15/tau**4)*(dt-dtv)**4)+ ((10/tau**3)*(dt-dtv)**3)
if fp < 1: #Transcion
p = JointTrajectoryPoint()
#
pv1 = ang1v*np.sin((vel_1v*dt)*2*np.pi) + ang1_offset
pn1 = ang1*np.sin((vel_1*dt)*2*np.pi) + ang1_offset
pvv1 = vel_1v*2*np.pi*ang1v*np.cos((vel_1v*(dt-pub_period))*2*np.pi)
pnv1 = vel_1*2*np.pi*ang1*np.cos((vel_1*(dt-pub_period))*2*np.pi)
pv2 = ang2v*np.sin((vel_2v*dt)*2*np.pi) + ang2_offset
pn2 = ang2*np.sin((vel_2*dt)*2*np.pi) + ang2_offset
pvv2 = vel_2v*2*np.pi*ang2v*np.cos((vel_2v*dt)*2*np.pi)
pnv2 = vel_2*2*np.pi*ang2*np.cos((vel_2*dt)*2*np.pi)
##Position#
mix1 = fp*pn1 + (1-fp)*pv1
mix2 = fp*pn2 + (1-fp)*pv2
##Vel##
mixv1 = fp*pnv1 + (1-fp)*pvv1
mixv2 = fp*pnv2 + (1-fp)*pvv2
i = mix1
j = mix2
k = mixv1
l = mixv2
p.positions = [i,j]
p.velocities = [k,l]
p.accelerations = [0,0]
p.time_from_start = rospy.Time.now()
rospy.loginfo("point trans \n %s ", p)
rospy.loginfo(fp)
pub.publish(p)
### graficar ###
pub_pv1.publish(pv1)
pub_pn1.publish(pn1)
pub_mix1.publish(mix1)
pub_fp.publish(fp)
###
else:
transition = False
else: #Estado Estable #Dentro de transicion OJO no sabemso si la funcion es monotonica
p = JointTrajectoryPoint()
i = ang1*np.sin((vel_1*dt)*2*np.pi) + ang1_offset
j = ang2*np.sin((vel_2*dt)*2*np.pi) + ang2_offset
k = vel_1*2*np.pi*ang1*np.cos((vel_1*(dt-pub_period))*2*np.pi)
l = vel_2*2*np.pi*ang2*np.cos((vel_2*dt)*2*np.pi)
vel_1v = vel_1
vel_2v = vel_2
dtv = dt
ang1v = ang1
ang2v = ang2
p.positions = [i,j]
p.velocities = [k,l]
p.accelerations = [0,0]
p.time_from_start = rospy.Time.now()
rospy.loginfo("point \n %s ", p)
pub.publish(p)
next_point_time = time.time() + pub_period
elif run_var == False:
start_time = time.time() + dt
def stage_6(self):
# Reverse turtlebot a specfic distance and lower arm at a specific velocity to target position
rospy.loginfo("Begin stage 6")
# Publisher to control the robot's speed
self.cmd_vel_1 = rospy.Publisher('turtlebot_1/cmd_vel_mux/input/navi',
Twist)
self.cmd_vel_2 = rospy.Publisher('turtlebot_2/cmd_vel_mux/input/navi',
Twist)
# Create a joint state publisher
self.arm_pub_1 = rospy.Publisher('turtlebot_1/arm_controller/command',
JointTrajectory)
self.arm_pub_2 = rospy.Publisher('turtlebot_2/arm_controller/command',
JointTrajectory)
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(1)
# How fast will we update the robot's movement? in Hz
rate = 10
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Define a joint trajectory message for support position
rospy.loginfo("Position arm in support position")
msg = JointTrajectory()
msg.header.seq = 1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '0'
msg.joint_names.append('joint_1')
msg.joint_names.append('joint_2')
msg.joint_names.append('joint_3')
msg.joint_names.append('joint_4')
msg.joint_names.append('joint_5')
# Define joint trajectory points
point = JointTrajectoryPoint()
point.positions = [0.0, 1.0, -1.0, 1.0, -1.8]
point.velocities = [2.0, 2.0, 20.0, 10.0, 1.0]
point.time_from_start = rospy.Duration(4.0)
msg.points.append(point)
# Define joint trajectory points
point = JointTrajectoryPoint()
point.positions = [0.0, 1.5, 0.0, 0.0, -1.8]
point.velocities = [10.0, 10.0, 10.0, 10.0, 1.0]
point.time_from_start = rospy.Duration(8.0)
msg.points.append(point)
# publish joint stage message
self.arm_pub_1.publish(msg)
self.arm_pub_2.publish(msg)
#rospy.sleep(3)
# Reverse turtlebot and open skirt door
# How fast will we update the robot's movement? in Hz
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.1 meters per second
linear_speed = 0.1
# Set the travel distance in meters
goal_distance = 0.49
# Set the odom frame
self.odom_frame = 'turtlebot_1/odom'
self.base_frame = 'turtlebot_1/base_footprint'
# Initialize the position variable as a Point type
position_1 = Point()
# Initialize the movement command
move_cmd_1 = Twist()
# Set the movement command to reverse motion
move_cmd_1.linear.x = -linear_speed
# Get the starting position values
(position_1, rotation) = get_odom(self)
x_start_1 = position_1.x
y_start_1 = position_1.y
# Keep track of the distance traveled
distance_1 = 0
####################################################################################################
# Set the odom frame
self.odom_frame = 'turtlebot_2/odom'
self.base_frame = 'turtlebot_2/base_footprint'
# Initialize the position variable as a Point type
position_2 = Point()
# Initialize the movement command
move_cmd_2 = Twist()
# Set the movement command to reverse motion
move_cmd_2.linear.x = -linear_speed
# Get the starting position values
(position_2, rotation) = get_odom(self)
x_start_2 = position_2.x
y_start_2 = position_2.y
# Keep track of the distance traveled
distance_2 = 0
rospy.loginfo("Begin moving turtlebot backwards")
# Enter the loop to move turtlebot
while distance_1 < goal_distance or distance_2 < goal_distance and not rospy.is_shutdown(
):
# Publish the Twist message and sleep 1 cycle
self.cmd_vel_1.publish(move_cmd_1)
self.cmd_vel_2.publish(move_cmd_2)
# sleep
r.sleep()
# Get the current position
# Set the odom frame
self.odom_frame = 'turtlebot_1/odom'
self.base_frame = 'turtlebot_1/base_footprint'
(position_1, rotation_1) = get_odom(self)
# Set the odom frame
self.odom_frame = 'turtlebot_2/odom'
self.base_frame = 'turtlebot_2/base_footprint'
(position_2, rotation_2) = get_odom(self)
# Compute the Euclidean distance from the start
distance_1 = sqrt(
pow((position_1.x - x_start_1), 2) +
pow((position_1.y - y_start_1), 2))
distance_2 = sqrt(
pow((position_2.x - x_start_2), 2) +
pow((position_2.y - y_start_2), 2))
# Check whether we need to stop one of the turtlebots early
if distance_1 >= goal_distance:
move_cmd_1 = Twist()
self.cmd_vel_1.publish(move_cmd_1)
if distance_2 >= goal_distance:
move_cmd_2 = Twist()
self.cmd_vel_2.publish(move_cmd_2)
# Stop the robot for good
rospy.loginfo("Stop turtlebots")
move_cmd_1 = Twist()
self.cmd_vel_1.publish(move_cmd_1)
move_cmd_2 = Twist()
self.cmd_vel_2.publish(move_cmd_2)
rospy.sleep(1)
def trajectory_test(self):
self.scene.remove_attached_object(self.eef_link, name="object")
self.scene.remove_world_object("object")
#Define how to perform trajectory planning for robot-to-human grasping. Cartesian = True indicates cartesian path planning,
#smoothed = True includes post-processing to apply time parameterization with external node.
cartesian = True
smoothed = True
while (not self.environment_set):
#Prepare the collision environment with respect to the marker
self.scene.remove_world_object("table")
self.scene.remove_world_object("right_restrict")
self.scene.remove_world_object("left_restrict")
self.scene.remove_world_object("top_restrict")
try:
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0, -0.26] looks before
jtp.time_from_start = rospy.Duration(1.0) head
jt.points.append(jtp)
self.head_cmd.publish(jt) #Look up to see the marker
rospy.sleep(1)
(trans,rot) = self.listener.lookupTransform('/base_footprint', '/arm_goal', rospy.Time(0))
#Collision object: table
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.header.frame_id = "aruco_marker_frame"
object_pose.pose.position.x=-self.object[0] - 0.05
object_pose.pose.position.y=-0.7
object_pose.pose.orientation.w=1.0
object_name = "table"
self.scene.add_box(object_name, object_pose, size=(0.1,2,2))
#Collision object: left block
obstacle_pose = geometry_msgs.msg.PoseStamped()
obstacle_pose.header.frame_id = "aruco_marker_frame"
obstacle_pose.pose.position.y=-0.5
obstacle_pose.pose.position.z = 1.7
obstacle_pose.pose.orientation.w=1.0
object_name = "left_restrict"
self.scene.add_box(object_name, obstacle_pose, size=(2,2,2))
#Collision object: top block
obstacle_pose = geometry_msgs.msg.PoseStamped()
obstacle_pose.header.frame_id = "aruco_marker_frame"
obstacle_pose.pose.position.x=1.5
obstacle_pose.pose.orientation.w=1.0
object_name = "top_restrict"
self.scene.add_box(object_name, obstacle_pose, size=(2,2,2))
#Collision object: right block
obstacle_pose = geometry_msgs.msg.PoseStamped()
obstacle_pose.header.frame_id = "aruco_marker_frame"
obstacle_pose.pose.position.y=-0.5
obstacle_pose.pose.position.z = -1.2
obstacle_pose.pose.orientation.w=1.0
object_name = "right_restrict"
self.scene.add_box(object_name, obstacle_pose, size=(2,2,2))
#Collision object: object
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.header.frame_id = "aruco_marker_frame"
object_pose.pose.position.x= -0.1
object_pose.pose.orientation.w=1.0
object_name = "object"
self.scene.remove_attached_object(self.eef_link, name="object")
self.scene.remove_world_object("object")
#Set initial object position
(trans2,rot2) = self.listener.lookupTransform('/base_footprint', '/place_goal', rospy.Time(0))
self.initial_pose = [trans2[0], trans2[1], trans2[2]]
self.initial_or = [rot2[0], rot2[1], rot2[2], rot2[3]]
print ("initial pose of object",trans2, rot2)
goal = PlayMotionGoal()
goal.motion_name = 'open_gripper'
goal.skip_planning = False
self.play_m_as.send_goal_and_wait(goal)
self.environment_set = True
print("ENVIRONMENT READY")
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0, -0.75]
jtp.time_from_start = rospy.Duration(1.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
marker = False
#Robot-to-human handover
if(self.action == 'robot'):
rospy.sleep(1)
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0, -0.56]
jtp.time_from_start = rospy.Duration(1.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
rospy.sleep(1.0)
touch_links=['gripper_link', 'gripper_left_finger_link', 'gripper_right_finger_link', 'gripper_grasping_frame']
#Detect marker
while(not marker):
print("Action started")
try:
marker = True
#Lookup transform between the base_footprint and arm_goal, which specifies the pregrasp position
(trans,rot) = self.listener.lookupTransform('/base_footprint', '/arm_goal', rospy.Time(0))
print("waiting")
self.write("Marker location " + str(trans))
self.write("Marker orientation " + str(rot))
#Add object as collision object
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.header.frame_id = "aruco_marker_frame"
object_pose.pose.position.x= -0.1
object_pose.pose.orientation.w=1.0
object_name = "object"
self.scene.remove_attached_object(self.eef_link, name=object_name)
self.scene.remove_world_object(object_name)
self.scene.add_box(object_name, object_pose, size=(self.object[0], self.object[1], self.object[2]))
goal = PlayMotionGoal()
goal.motion_name = 'open_gripper'
goal.skip_planning = False
self.play_m_as.send_goal_and_wait(goal)
x = self.marker_xtion[0]
y = self.marker_xtion[1]
#Track object
self.look_at(x,y, block=False)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
if not cartesian:
#Compute trajectory by specyfying end-effector goal directly
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = rot[0]
pose_target.orientation.y = rot[1]
pose_target.orientation.z = rot[2]
pose_target.orientation.w = rot[3]
pose_target.position.x = trans[0]
pose_target.position.y = trans[1]
pose_target.position.z = trans[2]
self.group.set_pose_target(pose_target)
self.message="moving"
self.pub.publish(self.message)
self.write("Starts moving")
init_movement = datetime.now()
plan_time_start = datetime.now()
self.group.set_planner_id("ESTkConfigDefault")
plan = self.group.plan()
plan_time_start = datetime.now()
plan_execute = datetime.now() - plan_time_start
print("Time for plan", plan_execute)
self.group.go()
rospy.sleep(20)
self.interpreter.execute("go forward 0.09")
print("completed path time", datetime.now() - plan_time_start)
else:
plan_time_start = datetime.now()
if smoothed: #Compute cartesian path planning with smoothing
send_data = Path()
waypoints = []
wpose = self.group.get_current_pose().pose
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = trans[0]
wpose.position.y = trans[1]
wpose.position.z = trans[2]
wpose.orientation.x = rot[0]
wpose.orientation.y = rot[1]
wpose.orientation.z = rot[2]
wpose.orientation.w = rot[3]
new_pose = PoseStamped()
new_pose.pose = wpose
send_data.poses.append(copy.deepcopy(new_pose))
wpose.position.x += 0.1
waypoints.append(copy.deepcopy(wpose))
new_pose = PoseStamped()
new_pose.pose = wpose
send_data.poses.append(copy.deepcopy(new_pose))
self.message="moving"
print self.message
self.pub.publish(self.message)
self.write("Starts moving")
init_movement = datetime.now()
while (self.plan_feedback == "Done"):
rospy.sleep(0.05)
msg = String()
msg.data = "request sent"
self.plan_done.publish(msg)
self.send_waypoints.publish(send_data)
print(self.plan_feedback)
while (self.plan_feedback!= "Done"):
rospy.sleep(0.05)
#Compute cartesian path planning without smoothing
else:
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.03, # eef_step
0.0,True) # jump_threshold
self.group.execute(plan, wait=True)
print("completed path time", datetime.now() - plan_time_start)
rospy.sleep(0.5)
def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
print("Opening the gripper")
whole_body.move_to_neutral()
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
print("Approach to the door")
grab_pose = geometry.multiply_tuples(
geometry.pose(x=hanlde_pos.position.x - HANDLE_TO_HAND_POS,
y=hanlde_pos.position.y,
z=hanlde_pos.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.008)
rospy.sleep(1.0)
switch.inactivate()
# Rotate the handle
whole_body.impedance_config = 'grasping'
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
# set the desired value
current_positions = [
latest_positions[name] for name in traj.joint_names
]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.55
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(5.0)
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0, 0, 1), 0.35)
whole_body.impedance_config = None
gripper.command(1.0)
whole_body.move_to_neutral()
res.success = True
except Exception as e:
rospy.logerr(e)
print "Failed to open door"
res.success = False
return res
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse)
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = current.keys()
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = current.values()
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(
wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.iteritems():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr(
"Invalid waypoint. Must contain valid name for named target or dict of joint angles."
)
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[
n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [
new_positions[n] for n in joint_names
]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
print "waiting to connect..."
client.wait_for_server()
print "connected! "
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
p1 = JointTrajectoryPoint()
p1.positions = [1.5, -0.2, -1.57, 0, 0, 0]
p1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p1.time_from_start = rospy.Duration(5.0)
g.trajectory.points.append(p1)
p2 = JointTrajectoryPoint()
p2.positions = [1.5, 0, -1.57, 0, 0, 0]
p2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p2.time_from_start = rospy.Duration(10.0)
g.trajectory.points.append(p2)
p3 = JointTrajectoryPoint()
p3.positions = [2.2, 0, -1.57, 0, 0, 0]
p3.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p3.time_from_start = rospy.Duration(15.0)
g.trajectory.points.append(p3)
client.send_goal(g)
def set_ee_cartesian_trajectory(self,
x=0,
y=0,
z=0,
roll=0,
pitch=0,
yaw=0,
moving_time=None,
wp_moving_time=0.5,
wp_accel_time=0.25,
wp_period=0.05):
if self.resp.num_joints < 6 and (y is not 0 or yaw is not 0):
rospy.loginfo(
"Please leave the 'y' and 'yaw' fields at '0' when working with arms that have less than 6dof."
)
return False
rpy = ang.rotationMatrixToEulerAngles(self.T_sb[:3, :3])
T_sy = np.identity(4)
T_sy[:3, :3] = ang.eulerAnglesToRotationMatrix([0.0, 0.0, rpy[2]])
T_yb = np.dot(mr.TransInv(T_sy), self.T_sb)
rpy[2] = 0.0
if (moving_time == None):
moving_time = self.moving_time
accel_time = self.accel_time
N = int(moving_time / wp_period)
inc = 1.0 / float(N)
joint_traj = JointTrajectory()
joint_positions = list(self.joint_positions)
for i in range(N + 1):
joint_traj_point = JointTrajectoryPoint()
joint_traj_point.positions = joint_positions
joint_traj_point.time_from_start = rospy.Duration.from_sec(
i * wp_period)
joint_traj.points.append(joint_traj_point)
if (i == N):
break
T_yb[:3, 3] += [inc * x, inc * y, inc * z]
rpy[0] += inc * roll
rpy[1] += inc * pitch
rpy[2] += inc * yaw
T_yb[:3, :3] = ang.eulerAnglesToRotationMatrix(rpy)
T_sd = np.dot(T_sy, T_yb)
theta_list, success = self.set_ee_pose_matrix(T_sd,
joint_positions,
False,
blocking=False)
if success:
joint_positions = theta_list
else:
rospy.loginfo(
"%.1f%% of trajectory successfully planned. Trajectory will not be executed."
% (i / float(N) * 100))
break
if success:
self.set_trajectory_time(wp_moving_time, wp_accel_time)
joint_traj.joint_names = self.resp.joint_names[:self.resp.
num_joints]
with self.js_mutex:
current_positions = list(
self.joint_states.position[self.waist_index:(
self.resp.num_joints + self.waist_index)])
joint_traj.points[0].positions = current_positions
joint_traj.header.stamp = rospy.Time.now()
self.pub_joint_traj.publish(joint_traj)
rospy.sleep(moving_time + wp_moving_time)
self.T_sb = T_sd
self.joint_positions = joint_positions
self.set_trajectory_time(moving_time, accel_time)
return success
def main():
os.makedirs(
os.path.join(assets_dir(), 'data',
'{}_ur3_train_data'.format(args.tag)))
rospy.init_node('send_joints') # create a publish node named send_joints
pub = rospy.Publisher('/trajectory_controller/command',
JointTrajectory,
queue_size=10)
# Create the topic message
traj = JointTrajectory()
traj.header = Header()
# Joint names for UR
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rate = rospy.Rate(10)
f_joint = open(
os.path.join(assets_dir(),
'data/{}_ur3_train_data/joint_data.txt'.format(args.tag)),
"a+")
f_end_effector = open(
os.path.join(
assets_dir(),
'data/{}_ur3_train_data/end_effector_data.txt'.format(args.tag)),
"a+")
iteration = 0
while not rospy.is_shutdown():
traj.header.stamp = rospy.Time.now()
pts = JointTrajectoryPoint()
random_pos_shoulder_lift_joint = np.random.uniform(
-3.14, 0) # set shoulder_lift_joint angle
random_pos = np.random.uniform(-3.14, 3.14,
size=(5)) # set other joints angle
random_pos = np.insert(random_pos, 1, random_pos_shoulder_lift_joint)
print("random_pos", random_pos)
pts.positions = random_pos
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)
time.sleep(5) # Delays for 5 seconds.
x_position, y_position, z_position = pos_main()
end_effector_position = np.array([x_position, y_position, z_position])
# pickle store data process......
# pickle.dump(random_pos, open(os.path.join(assets_dir(),
# 'data/{}_ur5_train_data/joint_angle_data.pkl'.format(args.tag)), 'wb'),
# protocol=pickle.HIGHEST_PROTOCOL)
# pickle.dump((x_position, y_position, z_position), open(os.path.join(assets_dir(),
# 'data/{}_ur5_train_data/end_effector_position_data.pkl'.format(args.tag)), 'wb'),
# protocol=pickle.HIGHEST_PROTOCOL)
print >> f_joint, random_pos
print >> f_end_effector, end_effector_position
iteration += 1
print("iteration", iteration)
if iteration == 1000:
break
def computeIKsPose(self, poselist, times, arm="arm", downsample_freq=None):
"""Giving a poselist (list of Pose) and times for every point compute it's iks and add it's times
if a value is given to downsample_freq then we will only compute IKs and so for the downsampled
rate of poses"""
rospy.loginfo("Computing " + str(len(poselist)) + " IKs")
fjt_goal = FollowJointTrajectoryGoal()
if arm == 'arm_torso':
fjt_goal.trajectory.joint_names = self.arm_torso
elif arm == 'arm_torso':
fjt_goal.trajectory.joint_names = self.arm_torso
elif arm == 'arm':
fjt_goal.trajectory.joint_names = self.arm
elif arm == 'arm':
fjt_goal.trajectory.joint_names = self.arm
# TODO: add other options
ik_answer = None
last_succesfull_ik_answer = None
if downsample_freq != None:
num_poses = 0
num_downsampled_poses = 0
for pose, time in zip(poselist, times):
if downsample_freq != None:
num_poses += 1
if num_poses % downsample_freq == 0:
num_downsampled_poses += 1
else: # if we are downsampling dont do IK calls if it's not one of the samples we want
continue
if last_succesfull_ik_answer != None:
ik_answer = self.getIkPose(
pose,
"arm",
previous_state=last_succesfull_ik_answer.solution)
else:
ik_answer = self.getIkPose(pose)
if DEBUG_MODE:
rospy.loginfo("Got error_code: " +
str(ik_answer.error_code.val) +
" which means: " +
moveit_error_dict[ik_answer.error_code.val])
if moveit_error_dict[ik_answer.error_code.val] == 'SUCCESS':
# We should check if the solution is very far away from joint config
# if so.. try again... being a generic solution i dont know how to manage this
last_succesfull_ik_answer = ik_answer
if DEBUG_MODE:
arrow = self.createArrowMarker(pose, ColorRGBA(0, 1, 0, 1))
self.ok_markers.markers.append(arrow)
jtp = JointTrajectoryPoint()
#ik_answer = GetConstraintAwarePositionIKResponse()
# sort positions and add only the ones of the joints we are interested in
positions = self.sortOutJointList(
fjt_goal.trajectory.joint_names,
ik_answer.solution.joint_state)
jtp.positions = positions
jtp.time_from_start = rospy.Duration(time)
fjt_goal.trajectory.points.append(jtp)
if DEBUG_MODE:
self.pub_ok_markers.publish(self.ok_markers)
else:
if DEBUG_MODE:
arrow = self.createArrowMarker(pose, ColorRGBA(1, 0, 0, 1))
self.fail_markers.markers.append(arrow)
self.pub_fail_markers.publish(self.fail_markers)
# Loop for a while to check if we get a solution on further checks?
if downsample_freq != None:
rospy.loginfo("From " + str(num_poses) +
" points we downsampled to " +
str(num_downsampled_poses) +
" and fjt_goal.trajectory.points has " +
str(fjt_goal.trajectory.points) + " points")
return fjt_goal
def takeAction(self, action):
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
print("debugX")
except (rospy.ServiceException) as e:
rospy.loginfo("[Walker unpause]: ROS unpause failed!")
joint = JointTrajectory()
points = JointTrajectoryPoint()
nameJoints = [
'l_shoulder_swing_joint', 'l_shoulder_lateral_joint',
'l_elbow_joint', 'r_shoulder_swing_joint',
'r_shoulder_lateral_joint', 'r_elbow_joint', 'l_hip_twist_joint',
'l_hip_lateral_joint', 'l_hip_swing_joint', 'l_knee_joint',
'l_ankle_swing_joint', 'l_ankle_lateral_joint',
'r_hip_twist_joint', 'r_hip_lateral_joint', 'r_hip_swing_joint',
'r_knee_joint', 'r_ankle_swing_joint', 'r_ankle_lateral_joint'
]
newAction = [
0, 0, 0, 0, 0, 0, 0, 0, action[0], action[1], action[2], 0, 0, 0,
action[2], action[3], action[4], 0
]
points.positions = newAction
points.time_from_start = rospy.Duration(0.01, 0)
joint.points = [points]
joint.joint_names = nameJoints
joint.header.stamp = rospy.Time.now()
self.pubJoint.publish(joint)
print("debugY")
reward = -0.1
currentTime = time.time()
if ((self.robotState.odometryX - self.robotState.lastOdometryX) >=
0.1) or ((self.robotState.odometryX -
self.robotState.lastOdometryX) < 0): #-0.001forward
deltaTime = currentTime - self.robotState.lastTime
reward += ((self.robotState.odometryX -
self.robotState.lastOdometryX)) * 30
print("[Ganho]:", reward)
print("[Distancia]:",
self.robotState.odometryX - self.robotState.lastOdometryX)
self.robotState.lastTime = currentTime
self.robotState.lastOdometryX = self.robotState.odometryX
if (self.robotState.contactFootR > 20
and self.robotState.contactFootL > 20
and self.robotState.odomPosZ <= 0.18):
reward += -100
self.robotState.done = True
self.robotState.fall = 1
if (np.abs(self.robotState.odomRotation.x) > 0.2
or np.abs(self.robotState.odomRotation.y) > 0.2
or np.abs(self.robotState.odomRotation.z) > 0.2):
reward += -80
self.robotState.done = True
self.robotState.fall = 1
print("debugB")
if self.robotState.odometryX > 9.0:
reward += 100
self.robotState.done = True
self.robotState.fall = 1
print("\33[92mREACHED TO THE END!\33[0m")
self.rate.sleep()
return reward, self.robotState.done
def on_enter(self, userdata):
self._param_error = False
self._request_failed = False
self._moveit_failed = False
self._success = False
# Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn(
'Unable to parse given SRDF parameter: /robot_description_semantic'
)
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib[
'name']:
robot = r
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' %
self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' %
self._config_name)
return 'param_error'
try:
self._joint_config = [
float(j.attrib['value']) for j in config.iter('joint')
]
self._joint_names = [
str(j.attrib['name']) for j in config.iter('joint')
]
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' %
str(e))
return 'param_error'
# Action Initialization
action_goal = ExecuteKnownTrajectoryRequest(
) # ExecuteTrajectoryGoal()
action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now(
) + rospy.Duration(0.2)
action_goal.trajectory.joint_trajectory.joint_names = [''] * len(
self._joint_names)
joint_trajectory_point = JointTrajectoryPoint()
action_goal.trajectory.joint_trajectory.points = [
joint_trajectory_point
]
joint_trajectory_point.positions = [0.0] * len(self._joint_names)
joint_trajectory_point.time_from_start = rospy.Duration(
self._duration)
action_goal.wait_for_execution = True
for i in range(len(self._joint_names)):
action_goal.trajectory.joint_trajectory.joint_names[
i] = self._joint_names[i]
action_goal.trajectory.joint_trajectory.points[0].positions[
i] = self._joint_config[i]
try:
self._response = self._client.call(self._action_topic,
action_goal)
Logger.loginfo(
"Execute Known Trajectory Service requested: \n" +
str(self._action_topic) + " " + str(action_goal))
except rospy.ServiceException as exc:
Logger.logwarn(
"Execute Known Trajectory Service did not process request: \n"
+ str(exc))
self._request_failed = True
return
import actionlib
import math
from control_msgs.msg import FollowJointTrajectoryAction
client = actionlib.SimpleActionClient('/panda_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
from control_msgs.msg import FollowJointTrajectoryGoal
goal = FollowJointTrajectoryGoal()
from trajectory_msgs.msg import JointTrajectory
joint_traj = JointTrajectory()
joint_traj.joint_names = ["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7"]
from trajectory_msgs.msg import JointTrajectoryPoint
point1 = JointTrajectoryPoint()
point2 = JointTrajectoryPoint()
point1.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795, 0.06015034910227879]
point1.velocities = [0., 0., 0., 0., 0., 0., 0.]
#point2.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879]
point2.positions = [0.33877080806309046, -1.5, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879]
point2.velocities = [0., 0., 0., 0., 0., 0., 0.]
point1.time_from_start = rospy.Duration(2.)
point2.time_from_start = rospy.Duration(4.)
from copy import deepcopy
joint_traj.points = [point1, point2, deepcopy(point1), deepcopy(point2)]
joint_traj.points[2].time_from_start += rospy.Duration(4.)
joint_traj.points[3].time_from_start += rospy.Duration(4.)
goal.trajectory = joint_traj
joint_traj.header.stamp = rospy.Time.now()+rospy.Duration(1.0)
client.send_goal_and_wait(goal)
def add_point(self, positions, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = [0.0] * len(self._goal.trajectory.joint_names)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def add_point(self, positions, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def create_grasp(self, pose, grasp_id):
"""
:type pose: Pose
pose of the gripper for the grasp
:type grasp_id: str
name for the grasp
:rtype: Grasp
"""
g = Grasp()
g.id = grasp_id
pre_grasp_posture = JointTrajectory()
pre_grasp_posture.header.frame_id = self._grasp_postures_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)
grasp_posture = copy.deepcopy(pre_grasp_posture)
grasp_posture.points[0].time_from_start = rospy.Duration(
self._time_pre_grasp_posture + self._time_grasp_posture)
jtpoint2 = JointTrajectoryPoint()
jtpoint2.positions = [
float(pos) for pos in self._gripper_grasp_positions.split()]
jtpoint2.time_from_start = rospy.Duration(
self._time_pre_grasp_posture +
self._time_grasp_posture + self._time_grasp_posture_final)
grasp_posture.points.append(jtpoint2)
g.pre_grasp_posture = pre_grasp_posture
g.grasp_posture = grasp_posture
header = Header()
header.frame_id = self._grasp_pose_frame_id # base_footprint
q = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
# Fix orientation from gripper_link to parent_link (tool_link)
fix_tool_to_gripper_rotation_q = quaternion_from_euler(
math.radians(self._fix_tool_frame_to_grasping_frame_roll),
math.radians(self._fix_tool_frame_to_grasping_frame_pitch),
math.radians(self._fix_tool_frame_to_grasping_frame_yaw)
)
q = quaternion_multiply(q, fix_tool_to_gripper_rotation_q)
fixed_pose = copy.deepcopy(pose)
fixed_pose.orientation = Quaternion(*q)
g.grasp_pose = PoseStamped(header, fixed_pose)
g.grasp_quality = self._grasp_quality
g.pre_grasp_approach = GripperTranslation()
g.pre_grasp_approach.direction.vector.x = self._pre_grasp_direction_x # NOQA
g.pre_grasp_approach.direction.vector.y = self._pre_grasp_direction_y # NOQA
g.pre_grasp_approach.direction.vector.z = self._pre_grasp_direction_z # NOQA
g.pre_grasp_approach.direction.header.frame_id = self._grasp_postures_frame_id # NOQA
g.pre_grasp_approach.desired_distance = self._grasp_desired_distance # NOQA
g.pre_grasp_approach.min_distance = self._grasp_min_distance
g.post_grasp_retreat = GripperTranslation()
g.post_grasp_retreat.direction.vector.x = self._post_grasp_direction_x # NOQA
g.post_grasp_retreat.direction.vector.y = self._post_grasp_direction_y # NOQA
g.post_grasp_retreat.direction.vector.z = self._post_grasp_direction_z # NOQA
g.post_grasp_retreat.direction.header.frame_id = self._grasp_postures_frame_id # NOQA
g.post_grasp_retreat.desired_distance = self._grasp_desired_distance # NOQA
g.post_grasp_retreat.min_distance = self._grasp_min_distance
g.max_contact_force = self._max_contact_force
g.allowed_touch_objects = self._allowed_touch_objects
return g
def functional(axis, angle):
pub_rosey = rospy.Publisher(
'/cyton_joint_trajectory_action_controller/command',
JointTrajectory, queue_size=10)
rospy.init_node('traj_maker', anonymous=True)
time.sleep(1)
axis = int(axis)
angle = float(angle)
rate = rospy.Rate(0.01)
while not rospy.is_shutdown():
# first way to define a point
traj_waypoint_1_rosey = JointTrajectoryPoint()
traj_waypoint_1_rosey.positions = [0,0,0,0,0,0,0]
traj_waypoint_1_rosey.time_from_start = Duration(2)
# second way to define a point
traj_waypoint_2_rosey = traj_waypoint_1_rosey
traj_waypoint_2_rosey.time_from_start = Duration(4)
traj_waypoint_2_rosey.positions[axis-1] = angle
time.sleep(1)
traj_waypoint_3_rosey = traj_waypoint_2_rosey
traj_waypoint_3_rosey.time_from_start = Duration(7)
#traj_waypoint_2_rosey = JointTrajectoryPoint(positions=[.31, -.051, .33, -.55, .28, .60,0],
#time_from_start = Duration(4))
#traj_waypoint_3_rosey = JointTrajectoryPoint(positions=[.14726, -.014151, .166507, -.33571, .395997, .38657,0],
#time_from_start = Duration(6))
#traj_waypoint_4_rosey = JointTrajectoryPoint(positions=[-.09309, .003150, .003559, .16149, .524427, -.1867,0],
#time_from_start = Duration(8))
#traj_waypoint_5_rosey = JointTrajectoryPoint(positions=[-.27752, .077886, -.1828, .38563, .682589, -.44665,0],
#time_from_start = Duration(10))
#traj_waypoint_6_rosey = JointTrajectoryPoint(positions=[0,0,0,0,0,0,0],time_from_start = Duration(12))
#debug in terminal
print traj_waypoint_2_rosey.positions
# making message
message_rosey = JointTrajectory()
# required headers
header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now())
message_rosey.header = header_rosey
# adding in joints
joint_names = ['shoulder_roll_joint', \
'shoulder_pitch_joint', 'shoulder_yaw_joint', 'elbow_pitch_joint', \
'elbow_yaw_joint', 'wrist_pitch_joint', 'wrist_roll_joint']
message_rosey.joint_names = joint_names
# appending up to 100 points
# ex. for i in enumerate(len(waypoints)): append(waypoints[i])
message_rosey.points.append(traj_waypoint_1_rosey)
message_rosey.points.append(traj_waypoint_2_rosey)
message_rosey.points.append(traj_waypoint_3_rosey)
#message_rosey.points.append(traj_waypoint_4_rosey)
#message_rosey.points.append(traj_waypoint_5_rosey)
#message_rosey.points.append(traj_waypoint_6_rosey)
# publishing to ROS node
pub_rosey.publish(message_rosey)
rate.sleep()
if rospy.is_shutdown():
break
def construct_trajectory_point(posture, duration):
trajectory_point = JointTrajectoryPoint()
trajectory_point.time_from_start = rospy.Duration.from_sec(float(duration))
for key in joint_trajectory.joint_names:
trajectory_point.positions.append(posture[key])
return trajectory_point
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
rospy.init_node('send_open_manipulator_arm_joint_angles')
pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=1)
controller_name = "arm_controller"
joint_names = rospy.get_param("/%s/joints" % controller_name)
rospy.loginfo("Joint names: %s" % joint_names)
rate = rospy.Rate(10)
trajectory_command = JointTrajectory()
trajectory_command.joint_names = joint_names
point = JointTrajectoryPoint()
#Joint names: ['joint1', 'joint2', 'joint3', 'joint4']
point.positions = [-0.6, 0.6, 0.1, -0.7]
point.velocities = [0.0, 0.0, 0.0, 0.0]
point.time_from_start = rospy.rostime.Duration(1, 0)
trajectory_command.points = [point]
while not rospy.is_shutdown():
trajectory_command.header.stamp = rospy.Time.now()
pub.publish(trajectory_command)
rate.sleep()
def generate_grasps(self, name, pose):
grasps = []
now = rospy.Time.now()
#for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(10.0)):
# Create place location:
angle = 0
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose = copy.deepcopy(pose)
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector.z = -0.2
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector.z = 0.2
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
# if name != "cylinder":
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
# else:
# #grasp.pre_grasp_approach.direction.vector.z = -0.05
# grasp.pre_grasp_approach.direction.vector.x = 0.1
# #grasp.post_grasp_retreat.direction.vector.z = 0.05
# grasp.post_grasp_retreat.direction.vector.x = -0.1
grasp.max_contact_force = 100
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.03)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
if name == "box":
traj.positions.append(0.57)
elif name == "sphere":
traj.positions.append(0.3)
else:
traj.positions.append(0.3)
#traj.velocities.append(0.2)
traj.effort.append(800)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
return grasps
def interpolated_ik_motion_planner_callback(self, req):
#names and angles for the joints in their desired order
joint_names = req.motion_plan_request.start_state.joint_state.name
start_angles = req.motion_plan_request.start_state.joint_state.position
#sanity-checking: joint_names and start_angles should be the same length, if any start_angles are specified
if start_angles and len(joint_names) != len(start_angles):
rospy.logerr(
"start_state.joint_state.name needs to be the same length as start_state.joint_state.position! Quitting"
)
return 0
#reorder the start angles to the order needed by IK
reordered_start_angles = []
#get the current joint states for the robot
#joint_states_msg = rospy.wait_for_message('joint_states', JointState, 10.0)
#if not joint_states_msg:
# rospy.logerr("unable to get joint_states message")
# return 0
#get the desired start angles for each IK arm joint in turn from start_state.joint_state.position
#(use the current angle if not specified)
for joint_name in self.ik_utils.joint_names:
#desired start angle specified
if joint_name in joint_names and start_angles:
index = joint_names.index(joint_name)
reordered_start_angles.append(start_angles[index])
else:
rospy.logerr("missing joint angle, can't deal")
return 0
#get additional desired joint angles (such as for the gripper) to pass through to IK
additional_joint_angles = []
additional_joint_names = []
for (ind, joint_name) in enumerate(joint_names):
if joint_name not in self.ik_utils.joint_names:
#rospy.loginfo("found %s"%joint_name)
additional_joint_angles.append(start_angles[ind])
additional_joint_names.append(joint_name)
IK_robot_state = None
if additional_joint_angles:
#rospy.loginfo("adding additional start angles for:"+str(additional_joint_names))
#rospy.loginfo("additional joint angles:"+str(additional_joint_angles))
IK_robot_state = RobotState()
IK_robot_state.joint_state.name = additional_joint_names
IK_robot_state.joint_state.position = additional_joint_angles
start_pose_stamped = self.ik_utils.run_fk(reordered_start_angles,
self.ik_utils.link_name)
#the desired goal position
goal_pos = req.motion_plan_request.goal_constraints.position_constraints[
0].position
#the frame that goal position is in
goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[
0].header.frame_id
#convert the position to base_link frame
goal_ps = self.add_header(PointStamped(), goal_pos_frame)
goal_ps.point = goal_pos
goal_pos_list = self.ik_utils.point_stamped_to_list(
goal_ps, 'base_link')
#the desired goal orientation
goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[
0].orientation
#the frame that goal orientation is in
goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[
0].header.frame_id
#convert the quaternion to base_link frame
goal_qs = self.add_header(QuaternionStamped(), goal_quat_frame)
goal_qs.quaternion = goal_quat
goal_quat_list = self.ik_utils.quaternion_stamped_to_list(
goal_qs, 'base_link')
#assemble the goal pose into a PoseStamped
goal_pose_stamped = self.add_header(PoseStamped(), 'base_link')
goal_pose_stamped.pose = Pose(Point(*goal_pos_list),
Quaternion(*goal_quat_list))
#get the ordered collision operations, if there are any
ordered_collision_operations = None #req.motion_plan_request.ordered_collision_operations
#if ordered_collision_operations.collision_operations == []:
# ordered_collision_operations = None
#get the link paddings, if there are any
link_padding = None #req.motion_plan_request.link_padding
#if link_padding == []:
# link_padding = None
#RUN! Check the Cartesian path for consistent, non-colliding IK solutions
(trajectory, error_codes) = self.ik_utils.check_cartesian_path(start_pose_stamped, \
goal_pose_stamped, reordered_start_angles, self.pos_spacing, self.rot_spacing, \
self.consistent_angle, self.collision_aware, self.collision_check_resolution, \
self.steps_before_abort, self.num_steps, ordered_collision_operations, \
self.start_from_end, IK_robot_state, link_padding)
#find appropriate velocities and times for the valid part of the resulting joint path (invalid parts set to 0)
#if we're searching from the end, keep the end; if we're searching from the start, keep the start
start_ind = 0
stop_ind = len(error_codes)
if self.start_from_end:
for ind in range(len(error_codes) - 1, 0, -1):
if error_codes[ind]:
start_ind = ind + 1
break
else:
for ind in range(len(error_codes)):
if error_codes[ind]:
stop_ind = ind
break
(times, vels) = self.ik_utils.trajectory_times_and_vels(
trajectory[start_ind:stop_ind], self.max_joint_vels,
self.max_joint_accs)
times = [0] * start_ind + times + [0] * (len(error_codes) - stop_ind)
vels = [[0] * 7] * start_ind + vels + [[0] * 7] * (len(error_codes) -
stop_ind)
rospy.logdebug("trajectory:")
for ind in range(len(trajectory)):
rospy.logdebug("error code " + str(error_codes[ind]) + " pos : " +
self.pplist(trajectory[ind]))
rospy.logdebug("")
for ind in range(len(trajectory)):
rospy.logdebug("time: " + "%5.3f " % times[ind] + "vels: " +
self.pplist(vels[ind]))
#the response
res = GetMotionPlanResponse()
#the arm joint names in the normal order, as spit out by IKQuery
res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:]
#a list of 7-lists of joint angles, velocities, and times for a trajectory that gets you from start to goal
#(all 0s if there was no IK solution for a point on the path)
res.trajectory.joint_trajectory.points = []
for i in range(len(trajectory)):
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = trajectory[i]
joint_trajectory_point.velocities = vels[i]
joint_trajectory_point.time_from_start = rospy.Duration(times[i])
res.trajectory.joint_trajectory.points.append(
joint_trajectory_point)
#a list of ArmNavigationErrorCodes messages, one for each trajectory point, with values as follows:
#ArmNavigationErrorCodes.SUCCESS (1): no problem
#ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided)
#ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-20): inconsistency in path between this point and the next point
#ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution)
#ArmNavigationErrorCodes.PLANNING_FAILED (0): aborted before getting to this point
error_code_dict = {0:ArmNavigationErrorCodes.SUCCESS, 1:ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED, \
2:ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED, 3:ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED, \
4:ArmNavigationErrorCodes.PLANNING_FAILED}
trajectory_error_codes = [
ArmNavigationErrorCodes(val=error_code_dict[error_code])
for error_code in error_codes
]
res.trajectory_error_codes = trajectory_error_codes
res.error_code.val = ArmNavigationErrorCodes.SUCCESS if max(
error_codes) == 0 else ArmNavigationErrorCodes.PLANNING_FAILED
# rospy.loginfo("trajectory:")
# for ind in range(len(trajectory)):
# rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
# rospy.loginfo("")
# for ind in range(len(trajectory)):
# rospy.loginfo("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind]))
return res
def publishAngles(self):
traj = JointTrajectory()
point = JointTrajectoryPoint()
traj.header.frame_id = "/"
# leg1
traj.joint_names.append("lf_hip_joint")
traj.joint_names.append("lf_upper_leg_joint")
traj.joint_names.append("lf_lower_leg_joint")
# point.positions.append(self.leg1_angles[0])
# point.positions.append(self.leg1_angles[1] - self.leg1_angles[2])
# point.positions.append(self.leg1_angles[3])
point.positions.append(self.leg1_angles[0])
point.positions.append((self.leg1_angles[1] + self.leg1_angles[2]))
point.positions.append(self.leg1_angles[3])
# leg2
traj.joint_names.append("rf_hip_joint")
traj.joint_names.append("rf_upper_leg_joint")
traj.joint_names.append("rf_lower_leg_joint")
# point.positions.append(self.leg2_angles[0])
# point.positions.append(self.leg2_angles[1] - self.leg2_angles[2])
# point.positions.append(self.leg2_angles[3])
point.positions.append(self.leg2_angles[0])
point.positions.append((self.leg2_angles[1] + self.leg2_angles[2]))
point.positions.append(self.leg2_angles[3])
# leg3
traj.joint_names.append("rh_hip_joint")
traj.joint_names.append("rh_upper_leg_joint")
traj.joint_names.append("rh_lower_leg_joint")
# point.positions.append(self.leg3_angles[0])
# point.positions.append(self.leg3_angles[1] - self.leg3_angles[2])
# point.positions.append(self.leg3_angles[3])
point.positions.append(self.leg3_angles[0])
point.positions.append((self.leg3_angles[1] + self.leg3_angles[2]))
point.positions.append(self.leg3_angles[3])
# leg4
traj.joint_names.append("lh_hip_joint")
traj.joint_names.append("lh_upper_leg_joint")
traj.joint_names.append("lh_lower_leg_joint")
# point.positions.append(self.leg4_angles[0])
# point.positions.append(self.leg4_angles[1] - self.leg4_angles[2])
# point.positions.append(self.leg4_angles[3])
point.positions.append(self.leg4_angles[0])
point.positions.append((self.leg4_angles[1] + self.leg4_angles[2]))
point.positions.append(self.leg4_angles[3])
for i in range(12):
point.velocities.append(0)
point.accelerations.append(0)
# point.positions.append(0)
point.time_from_start = rospy.Duration(0, 2e8)
traj.points.append(point)
traj.header.stamp = rospy.Time.now()
# print(traj)
self.pub.publish(traj)
self.rate.sleep()
names = ['motor_one', 'motor_two']
if __name__ == '__main__':
rospy.init_node('pololu_action_example_client')
client = actionlib.SimpleActionClient('pololu_trajectory_action_server',
pololu_trajectoryAction)
client.wait_for_server()
goal = pololu_trajectoryGoal()
traj = goal.joint_trajectory
traj.header.stamp = rospy.Time.now()
traj.joint_names.append(names[0])
traj.joint_names.append(names[1])
pts = JointTrajectoryPoint()
pts.time_from_start = rospy.Duration(2.0)
pts.positions.append(-0.77)
pts.positions.append(0.77)
pts.velocities.append(1.0)
pts.velocities.append(1.0)
traj.points.append(pts)
pts = JointTrajectoryPoint()
pts.time_from_start = rospy.Duration(3.0)
pts.positions.append(0.77)
pts.positions.append(-0.77)
pts.velocities.append(1.0)
pts.velocities.append(1.0)
traj.points.append(pts)
# Fill in the goal here
client.send_goal(goal)
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
self._get_trajectory_parameters(joint_names, goal)
num_joints = len(joint_names)
trajectory_points = goal.trajectory.points
pnt_times = [0.0] * len(trajectory_points)
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (action_name, ))
self._server.set_aborted()
return
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
trajectory_points[-1].velocities = [0.0] * len(joint_names)
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
if (num_points == 1) or (pnt_times[0] > 0.0):
# Add current position as trajectory point
first_trajectory_point = JointTrajectoryPoint()
first_trajectory_point.positions = self._get_current_position(
joint_names)
# To preserve desired velocities and accelerations, copy them to the first
# trajectory point if the trajectory is only 1 point.
first_trajectory_point.time_from_start = rospy.Duration(0)
trajectory_points.insert(0, first_trajectory_point)
num_points = len(trajectory_points)
for i in range(num_points):
trajectory_points[i].velocities = [0.0] * len(joint_names)
trajectory_points[i].accelerations = [0.0] * len(joint_names)
for i in range(1, num_points):
for j in range(num_joints):
if ((pnt_times[i] - pnt_times[i - 1]) > 0.0):
trajectory_points[i].velocities[j] = (
trajectory_points[i].positions[j] -
trajectory_points[i - 1].positions[j]) / (
pnt_times[i] - pnt_times[i - 1])
"""
Wait for the specified execution time, if not provided use now
"""
start_time = goal.trajectory.header.stamp.to_sec()
now = rospy.get_time()
if start_time == 0.0:
start_time = rospy.get_time()
while start_time > now:
now = rospy.get_time()
"""
Loop until end of trajectory time. Provide a single time step
of the control rate past the end to ensure we get to the end.
Keep track of current indices for spline segment generation
"""
control_rate = rospy.Rate(self._trajectory_control_rate)
now_from_start = rospy.get_time() - start_time
end_time = trajectory_points[-1].time_from_start.to_sec()
last_idx = 0
slewed_pos = self._get_current_position(joint_names)
self._command_joints(joint_names, deepcopy(trajectory_points[0]))
while (now_from_start < end_time and not rospy.is_shutdown()
and self.robot_is_enabled()):
now = rospy.get_time()
now_from_start = now - start_time
for i in range(num_points):
if (pnt_times[i] >= now_from_start):
idx = i
break
if (idx >= num_points):
idx = num_points - 1
for j in range(num_joints):
slewed_pos[j] += trajectory_points[idx].velocities[j] * (
1.0 / self._trajectory_control_rate)
point = deepcopy(trajectory_points[idx])
point.positions = slewed_pos
"""
Command Joint Position, Velocity, Acceleration
"""
command_executed = self._command_joints(joint_names, point)
self._update_feedback(deepcopy(point), joint_names, now_from_start)
"""
Break the loop if the command cannot be executed
"""
if not command_executed:
return
control_rate.sleep()
"""
Keep trying to meet goal until goal_time constraint expired
"""
last = trajectory_points[-1]
last_time = trajectory_points[-1].time_from_start.to_sec()
end_angles = dict(zip(joint_names, last.positions))
while (now_from_start < (last_time + self._goal_time)
and not rospy.is_shutdown() and self.robot_is_enabled()):
if not self._command_joints(joint_names, last):
return
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
"""
Verify goal constraint
"""
result = self._check_goal_state(joint_names, last)
if result is True:
rospy.loginfo("%s: Joint Trajectory Action Succeeded" %
self._action_name)
self._result.error_code = self._result.SUCCESSFUL
self._server.set_succeeded(self._result)
elif result is False:
rospy.logerr("%s: Exceeded Max Goal Velocity Threshold" %
self._action_name)
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
else:
rospy.logerr("%s: Exceeded Goal Threshold Error %s" %
(self._action_name, result))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
self._command_stop()
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint',
'arm_5_joint', 'arm_6_joint', 'arm_7_joint'
]
jtp = JointTrajectoryPoint()
data = rospy.get_param("mymotions")
points = data.get('play_motion').get('motions').get('grasp17').get(
'points')[0]
jtp.positions = points.get('positions')
jtp.velocities = [0.12, 0.12, 0.12, 0.12, 0.12, 0.12, 0.12]
jtp.time_from_start = rospy.Duration(1.5) #I can set the duration here
goal.trajectory.points.append(jtp)
client.send_goal(goal)
client.wait_for_result()
pub.publish('not moving')
elif confirmcheck == '2':
message = 'stop'
stop_pub.publish(message)
elif confirmcheck == '3':
message = 'go'
stop_pub.publish(message)
elif confirmcheck == '4':
# a publisher for arm movement
client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
# prepare a joint trajectory
msg = JointTrajectory()
msg.joint_names = servos
msg.points = list()
# tucking or untucking?
if len(sys.argv) > 1 and sys.argv[1] == "u":
point = JointTrajectoryPoint()
point.positions = utcked
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(3.0)
msg.points.append(point)
else:
point = JointTrajectoryPoint()
point.positions = utcked
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 = tucked
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(8.0)
msg.points.append(point)
# execute
msg.header.stamp = rospy.Time.now()
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))
velocity = 2 * math.pi / float(time_range) * angle_range * math.cos(
2 * math.pi * (i + 1) / float(N))
for j in xrange(6):
positions.append(position)
velocities.append(velocity)
point.positions = positions
point.velocities = velocities
points.append(point)
traj.points = points
# rospy.loginfo(traj)
trajPub.publish(traj)
rospy.loginfo('published?')
def controller(ps, motorOn, control_info):
#Controller for creating the command message to pass to the arm_controller
# motorOn [in] = specifier for whether to actuate the system (convenience feature for testing)
# control_info [in]= control structure specifying the relevant control parameters
start_time = rospy.Time.now()
time_from_start = 0.
ps.final_time = control_info['tf']
if ps.startBag:
ps.bagging = True
#MinJerk trajectory type specifies waypoints to move to with zero velocity and accelration
#boundary conditions
if control_info['type'] == "MinJerk":
waypoints = np.array(
[[-0.02, -0.02, -0.02, 0.365, 0.365, 0.365, 0.0, 0.0],
[0.49, 0.49, 0.49, 0.2175, 0.2175, 0.2175, 0.395, 0.395],
[0.1, -0.02, 0.1, 0.1, -0.02, 0.1, 0.1, 0.1],
[0., 0.0, 0., np.pi / 2, np.pi / 2, 0.0, 0.0, 0.0]])
#Specify time interval for each segment
time_array = np.array([2., 2., 2., 2., 2., 2., 2., 2.])
#Compute the total time needed
tf = np.sum(time_array)
#Use the inverseKinematics function to generate the starting joint configuration
initial_angles = TG.inverseKinematics(0.0, 0.395, 0.1, 0)
#Compute the minimum jerk constants
joint_const = TG.minJerkSetup_now(initial_angles,
tf,
waypoints,
t_array=time_array)
#Loop for the duration of the control interval
while control_info['tf'] - time_from_start > 0:
deflection = ps.queue.deflection[:, -1]
temperature = ps.queue.windingTempFlt[:, -1]
cmd = model_learning.msg.CommandML()
trajectory = JointTrajectory()
point = JointTrajectoryPoint()
trajectory.header.stamp = rospy.Time.now()
point.time_from_start = trajectory.header.stamp - start_time
time_from_start = (point.time_from_start.secs +
point.time_from_start.nsecs / 1000000000.)
#Compute the next time step for the minimum jerk trajectory
if control_info['type'] == "MinJerk":
pos_v, vel_v, accel_v = TG.minJerkStep_now(time_from_start,
tf,
waypoints,
joint_const,
t_array=time_array)
#For each of the joints, populate the command trajectory
for joint in range(0, 5):
if joint == 0 or joint == 1 or joint == 2 or joint == 3 or joint == 4:
if control_info['type'] == "Circle":
pos_v, vel_v, accel_v = TG.generateJointTrajectory_now(
time_from_start)
pos = pos_v[joint]
vel = vel_v[joint]
accel = accel_v[joint]
elif control_info['type'] == "MinJerk":
pos = pos_v[joint]
vel = vel_v[joint]
accel = accel_v[joint]
else:
pos, vel, accel = 0., 0., 0.
point.positions.append(pos)
point.velocities.append(vel)
point.accelerations.append(accel)
first_joint = 0
eps = [0., 0., 0., 0., 0.]
#Popuylate the command message with appropriate values
cmd.epsTau = eps
cmd.jointTrajectory.points.append(point)
cmd.motorOn = float(motorOn)
cmd.controlType = 1. # Designates torque control
cmd.closedLoop = control_info['closedLoop']
cmd.feedforward = control_info['feedforward']
cmd.pos_gain = control_info['p_gain']
cmd.vel_gain = control_info['v_gain']
ps.traj_pub.publish(cmd)
if ps.startBag:
ps.startBag = False
ps.bagging = False
time.sleep(0.001)
ps.bag.close()
def joint_angle_client():
#inhibitWalkSrv = rospy.ServiceProxy("inhibit_walk", std_srvs.srv.Empty)
#uninhibitWalkSrv = rospy.ServiceProxy("uninhibit_walk", std_srvs.srv.Empty)
client = actionlib.SimpleActionClient("joint_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction)
stiffness_client = actionlib.SimpleActionClient("joint_stiffness_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction)
angle_client = actionlib.SimpleActionClient("joint_angles_action", naoqi_bridge_msgs.msg.JointAnglesWithSpeedAction)
rospy.loginfo("Waiting for joint_trajectory and joint_stiffness servers...")
client.wait_for_server()
stiffness_client.wait_for_server()
angle_client.wait_for_server()
rospy.loginfo("Done.")
#inhibitWalkSrv()
try:
goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal()
# move head: single joint, multiple keypoints
goal.trajectory.joint_names = ["HeadYaw"]
goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0), positions = [1.0]))
goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.0), positions = [-1.0]))
goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.5), positions = [0.0]))
rospy.loginfo("Sending goal...")
client.send_goal(goal)
client.wait_for_result()
result = client.get_result()
rospy.loginfo("Results: %s", str(result.goal_position.position))
# Test for preemption
rospy.loginfo("Sending goal again...")
client.send_goal(goal)
rospy.sleep(0.5)
rospy.loginfo("Preempting goal...")
client.cancel_goal()
client.wait_for_result()
if client.get_state() != GoalStatus.PREEMPTED or client.get_result() == result:
rospy.logwarn("Preemption does not seem to be working")
else:
rospy.loginfo("Preemption seems okay")
# crouching pose: single keypoint, multiple joints:
goal.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
goal.trajectory.points = [point]
rospy.loginfo("Sending goal...")
client.send_goal(goal)
client.wait_for_result()
rospy.loginfo("Getting results...")
result = client.get_result()
print "Result:", ', '.join([str(n) for n in result.goal_position.position])
# multiple joints, multiple keypoints:
goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"]
goal.trajectory.points = []
goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5),
positions = [1.0, 1.0]))
goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0),
positions = [1.0, 0.0]))
goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.5),
positions = [0.0, 0.0]))
rospy.loginfo("Sending goal...")
client.send_goal(goal)
client.wait_for_result()
rospy.loginfo("Getting results...")
result = client.get_result()
print "Result:", ', '.join([str(n) for n in result.goal_position.position])
# multiple joints, single keypoint:
goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"]
goal.trajectory.points = []
goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5),
positions = [0.5, 0.5]))
rospy.loginfo("Sending goal...")
client.send_goal(goal)
client.wait_for_result()
rospy.loginfo("Getting results...")
result = client.get_result()
print "Result:", ', '.join([str(n) for n in result.goal_position.position])
# Control of joints with relative speed
angle_goal = naoqi_bridge_msgs.msg.JointAnglesWithSpeedGoal()
angle_goal.joint_angles.relative = False
angle_goal.joint_angles.joint_names = ["HeadYaw", "HeadPitch"]
angle_goal.joint_angles.joint_angles = [1.0, 0.0]
angle_goal.joint_angles.speed = 0.2
rospy.loginfo("Sending joint angles goal...")
angle_client.send_goal_and_wait(angle_goal)
result = angle_client.get_result()
rospy.loginfo("Angle results: %s", str(result.goal_position.position))
# Test for preemption
angle_goal.joint_angles.joint_angles = [-1.0, 0.0]
angle_goal.joint_angles.speed = 0.05
rospy.loginfo("Sending goal again...")
angle_client.send_goal(angle_goal)
rospy.sleep(0.5)
rospy.loginfo("Preempting goal...")
angle_client.cancel_goal()
angle_client.wait_for_result()
if angle_client.get_state() != GoalStatus.PREEMPTED or angle_client.get_result() == result:
rospy.logwarn("Preemption does not seem to be working")
else:
rospy.loginfo("Preemption seems okay")
# Test stiffness actionlib
stiffness_goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal()
stiffness_goal.trajectory.joint_names = ["Body"]
stiffness_goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [1.0]))
rospy.loginfo("Sending stiffness goal...")
stiffness_client.send_goal(stiffness_goal)
stiffness_client.wait_for_result()
result = stiffness_client.get_result()
rospy.loginfo("Stiffness results: %s", str(result.goal_position.position))
# Test for preemption
stiffness_goal.trajectory.points = [JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [0.0])]
rospy.loginfo("Sending goal again...")
stiffness_client.send_goal(stiffness_goal)
rospy.sleep(0.25)
rospy.loginfo("Preempting goal...")
stiffness_client.cancel_goal()
stiffness_client.wait_for_result()
if stiffness_client.get_state() != GoalStatus.PREEMPTED or stiffness_client.get_result() == result:
rospy.logwarn("Preemption does not seem to be working")
else:
rospy.loginfo("Preemption seems okay")
finally:
pass #uninhibitWalkSrv()
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
# Load parameters for trajectory
self._get_trajectory_parameters(joint_names, goal)
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (self._action_name,))
self._server.set_aborted()
return
rospy.loginfo("%s: Executing requested joint trajectory" %
(self._action_name,))
rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
control_rate = rospy.Rate(self._control_rate)
dimensions_dict = self._determine_dimensions(trajectory_points)
if num_points == 1:
# Add current position as trajectory point
first_trajectory_point = JointTrajectoryPoint()
first_trajectory_point.positions = self._get_current_position(joint_names)
# To preserve desired velocities and accelerations, copy them to the first
# trajectory point if the trajectory is only 1 point.
if dimensions_dict['velocities']:
first_trajectory_point.velocities = deepcopy(trajectory_points[0].velocities)
if dimensions_dict['accelerations']:
first_trajectory_point.accelerations = deepcopy(trajectory_points[0].accelerations)
first_trajectory_point.time_from_start = rospy.Duration(0)
trajectory_points.insert(0, first_trajectory_point)
num_points = len(trajectory_points)
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
if dimensions_dict['velocities']:
trajectory_points[-1].velocities = [0.0] * len(joint_names)
if dimensions_dict['accelerations']:
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
try:
b_matrix = self._compute_bezier_coeff(joint_names,
trajectory_points,
dimensions_dict)
except Exception as ex:
rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"
" arm with error \"{2}: {3}\"").format(
self._action_name,
self._name,
type(ex).__name__, ex))
self._server.set_aborted()
return
# Wait for the specified execution time, if not provided use now
start_time = goal.trajectory.header.stamp.to_sec()
if start_time == 0.0:
start_time = rospy.get_time()
baxter_dataflow.wait_for(
lambda: rospy.get_time() >= start_time,
timeout=float('inf')
)
# Loop until end of trajectory time. Provide a single time step
# of the control rate past the end to ensure we get to the end.
# Keep track of current indices for spline segment generation
now_from_start = rospy.get_time() - start_time
end_time = trajectory_points[-1].time_from_start.to_sec()
while (now_from_start < end_time and not rospy.is_shutdown() and
self.robot_is_enabled()):
#Acquire Mutex
now = rospy.get_time()
now_from_start = now - start_time
idx = bisect.bisect(pnt_times, now_from_start)
#Calculate percentage of time passed in this interval
if idx >= num_points:
cmd_time = now_from_start - pnt_times[-1]
t = 1.0
elif idx >= 0:
cmd_time = (now_from_start - pnt_times[idx-1])
t = cmd_time / (pnt_times[idx] - pnt_times[idx-1])
else:
cmd_time = 0
t = 0
point = self._get_bezier_point(b_matrix, idx,
t, cmd_time,
dimensions_dict)
# Command Joint Position, Velocity, Acceleration
command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict)
self._update_feedback(deepcopy(point), joint_names, now_from_start)
# Release the Mutex
if not command_executed:
return
control_rate.sleep()
# Keep trying to meet goal until goal_time constraint expired
last = trajectory_points[-1]
last_time = trajectory_points[-1].time_from_start.to_sec()
end_angles = dict(zip(joint_names, last.positions))
def check_goal_state():
for error in self._get_current_error(joint_names, last.positions):
if (self._goal_error[error[0]] > 0
and self._goal_error[error[0]] < math.fabs(error[1])):
return error[0]
if (self._stopped_velocity > 0.0 and
max([abs(cur_vel) for cur_vel in self._get_current_velocities(joint_names)]) >
self._stopped_velocity):
return False
else:
return True
while (now_from_start < (last_time + self._goal_time)
and not rospy.is_shutdown() and self.robot_is_enabled()):
if not self._command_joints(joint_names, last, start_time, dimensions_dict):
return
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names,
now_from_start)
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names,
now_from_start)
# Verify goal constraint
result = check_goal_state()
if result is True:
rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" %
(self._action_name, self._name))
self._result.error_code = self._result.SUCCESSFUL
self._server.set_succeeded(self._result)
elif result is False:
rospy.logerr("%s: Exceeded Max Goal Velocity Threshold for %s arm" %
(self._action_name, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
else:
rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" %
(self._action_name, result, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
self._command_stop(goal.trajectory.joint_names, end_angles, start_time, dimensions_dict)
def move(self):
client = actionlib.SimpleActionClient('pololu_trajectory_action_server', pololu_trajectoryAction)
client.wait_for_server()
self._image_subs = rospy.Subscriber("detectedObjects", Objects, self.im_callback);
self._sb = rospy.Subscriber("/pololu/motor_states", MotorStateList, self.IR_callback);
while(not rospy.is_shutdown()):
(objectDetected, area, imTime) = ImageObjects.getDetectedObject(self);
(frontDistance, sideDistance, frontSlope, sideSlope, iRTime) = IR_Subscriber.getIRDistances(self);
if self._lastIRTime == iRTime: #or self._lastImageTime == imTime:
continue;
else:
self._lastImageTime = imTime;
self._lastIRTime = iRTime;
deltaDistance = frontDistance, sideDistance
print "frontDistance: ", frontDistance," sideDistance: ",sideDistance," _lastIRTime: ",self._lastIRTime #, objectDetected," ", area, " _lastImageTime:", self._lastImageTime,
currTime = rospy.get_time()
goal = pololu_trajectoryGoal()
traj=goal.joint_trajectory
traj.header.stamp = rospy.Time.now()
traj.joint_names.append(names[0])
pts=JointTrajectoryPoint()
pts.time_from_start=rospy.Duration(0)
if objectDetected != "None" and not self._rollingBallTrajectoryActive:
if objectDetected == "Rolling_ball":
print "Detected Rolling Ball."
self.initiateTrajectoryToAvoidObstacle(self._lastIRTime);
else:
print "Stop sign."
self.stop(pts);
elif self._rollingBallTrajectoryActive:
if (self._lastIRTime.secs - self._rollingBallTrajectoryStartTime) > 3:
self._rollingBallTrajectoryActive = False;
setOriginalThresholdValues();
probablyTurning = False;
if frontDistance >= INFINITY_DIST: # No wall in front
if self._turningRight or self._turningLeft: #Now there is no wall in front. So, set "turningRight" to False.
self._turningRight = False;
self._turningLeft = False;
if (currTime - self._lastTurningTime) <= 5.0 and sideDistance >= INFINITY_DIST:
sideDistance = Min_side_threshold_dist-1.0;
if sideDistance > Max_side_threshold_dist:
self.moveMotorRight(sideDistance,pts)
elif sideDistance < Min_side_threshold_dist:
self.moveMotorLeft(sideDistance, pts)
else:
self.moveMotorCenter(pts, sideSlope, currTime)
else:
if frontDistance < Min_front_threshold_dist: #and (currTime - self._lastTurningTime) > 3.0:
if self._turningRight:
self.moveMotorRight(Max_right_limit,pts);
else:
self.moveMotorLeft(Max_left_limit,pts);
self._turningLeft = True;
self._lastTurningTime = currTime; #Save turning time
elif sideDistance > Max_side_threshold_dist:
if sideDistance >= INFINITY_DIST: #and (currTime - self._lastTurningTime) > 3.0: #If no wall is detected by right side sensor, then take right turn
self._turningRight = True;
self._lastTurningTime = currTime; #Save turning time
self.moveMotorRight(sideDistance, pts)
elif sideDistance < Min_side_threshold_dist:
self.moveMotorLeft(sideDistance, pts)
else:
self.moveMotorCenter(pts, sideSlope, currTime)
probablyTurning = True;
pts.velocities.append(1.0)
traj.joint_names.append(names[1]) #DC Motor
speed = 0.0;
if probablyTurning:
speed = 0.43 - self._speedOffset;
self._speedOffset += 0.003
else:
speed = MAX_SPEED + ((MIN_SPEED - MAX_SPEED)*self._speedOffset/100.0)
self._speedOffset = 0.0
#speed = MIN_SPEED if speed > MIN_SPEED else speed;
speed = MAX_SPEED if speed < MAX_SPEED else speed;
print "DC Motor Speed: ", speed
pts.positions.append(speed);
pts.velocities.append(1.0)
print "======================================================================="
traj.points.append(pts)
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(0.001))
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()
