def pickGoalCb(userdata, goal):
goal.target_name = userdata.object_name
goal.group_name = "arm"
goal.end_effector = "gripper"
grasp = moveit_msgs.Grasp()
grasp.id = "front_grasp"
grasp.pre_grasp_posture.header.stamp = rospy.Time.now()
grasp.pre_grasp_posture.header.frame_id = "gripper_link"
grasp.pre_grasp_posture.joint_names.append("gripper")
grasp_opened = trajectory_msgs.JointTrajectoryPoint()
grasp_opened.positions.append(1.3)
grasp_opened.velocities.append(0.0)
grasp_opened.accelerations.append(0.0)
grasp.pre_grasp_posture.points.append(grasp_opened)
grasp.grasp_posture.header.stamp = grasp.pre_grasp_posture.header.stamp
grasp.grasp_posture.header.frame_id = grasp.pre_grasp_posture.header.frame_id
grasp.grasp_posture.joint_names.append("gripper")
grasp_closed = trajectory_msgs.JointTrajectoryPoint()
grasp_closed.positions.append(0.2)
grasp_closed.velocities.append(0.0)
grasp_closed.accelerations.append(0.0)
grasp.grasp_posture.points.append(grasp_closed)
grasp.grasp_pose = userdata.object_pose
print 'grasp pose'
print grasp.grasp_pose
grasp.grasp_quality = 1.0
grasp.pre_grasp_approach.direction.header.stamp = grasp.pre_grasp_posture.header.stamp
grasp.pre_grasp_approach.direction.header.frame_id = "palm_link"
grasp.pre_grasp_approach.direction.vector.x = 1.0
grasp.pre_grasp_approach.direction.vector.y = 0.0
grasp.pre_grasp_approach.direction.vector.z = 0.0
grasp.pre_grasp_approach.desired_distance = 0.10
grasp.pre_grasp_approach.min_distance = 0.08
grasp.post_grasp_retreat.direction.header = grasp.pre_grasp_approach.direction.header
grasp.post_grasp_retreat.direction.vector.x = 0.0
grasp.post_grasp_retreat.direction.vector.y = 1.0
grasp.post_grasp_retreat.direction.vector.z = 0.0
grasp.post_grasp_retreat.desired_distance = 0.07
grasp.post_grasp_retreat.min_distance = 0.05
grasp.max_contact_force = 0.0 # disabled
# grasp.allowed_touch_objects.append("all") # optional
goal.possible_grasps.append(grasp)
goal.allow_gripper_support_collision = False
goal.attached_object_touch_links = [
'finger_left_knuckle_1_link', 'finger_left_knuckle_2_link',
'finger_left_tip_link', 'finger_right_knuckle_1_link',
'finger_right_knuckle_2_link', 'finger_right_tip_link'
]
goal.minimize_object_distance = False
# goal.path_constraints = ...
goal.planner_id = ""
goal.allowed_touch_objects.append("all")
goal.allowed_planning_time = 20.0
# goal.planning_options = ...
return goal
def to_ros_plan(t, p, v=None, a=None):
"""Convert a series of time stamps and positions to ros MoveItMsg.RobotTrajectory msg.
Note that the msg contains no joint name, which need to be added explicitly.
:param t: timestamp of shape N
:param p: way point positions of shape [dim, N]
:param v: way point velocities of shape [dim, N], could be all 0
:param a: way point accelerations of shape [dim, N], could be all 0
:return: MoveItMsg.RobotTrajectory with joint names be empty
"""
msg = MoveItMsg.RobotTrajectory()
way_point_num = t.size
dim = p.shape[0]
zero_list = np.zeros(dim).tolist()
for w in range(way_point_num):
if w == 0:
continue # omit the starting point identical to the current pose
wpt = TrajectoryMsg.JointTrajectoryPoint()
wpt.positions = list(p[:, w])
wpt.velocities = zero_list if v is None else list(v[:, w])
wpt.accelerations = zero_list if a is None else list(a[:, w])
wpt.time_from_start = rospy.Duration.from_sec(t[w])
msg.joint_trajectory.points.append(wpt)
return msg
def to_joint_trajectory_msg(self):
"""
Create trajectory msg for the publisher.
:returns
a ROS msg for the joint trajectory
"""
joint_trajectory_msg = trajectory_msg.JointTrajectory()
joint_trajectory_msg.joint_names = [
joint.name for joint in self.joints
]
timestamps = self.get_unique_timestamps()
for timestamp in timestamps:
joint_trajectory_point = trajectory_msg.JointTrajectoryPoint()
joint_trajectory_point.time_from_start = timestamp.to_msg()
for joint in self.joints:
interpolated_setpoint = joint.get_interpolated_setpoint(
timestamp)
joint_trajectory_point.positions.append(
interpolated_setpoint.position)
joint_trajectory_point.velocities.append(
interpolated_setpoint.velocity)
joint_trajectory_msg.points.append(joint_trajectory_point)
return joint_trajectory_msg
def goto_joint_positions(self, positions_goal):
positions_cur = self.get_joint_positions()
assert len(positions_goal) == len(positions_cur)
duration = norm(
(r_[positions_goal] - r_[positions_cur]) / self.vel_limits,
ord=inf)
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jtp = tm.JointTrajectoryPoint()
jtp.positions = positions_goal
jtp.velocities = zeros(len(positions_goal))
jtp.time_from_start = rospy.Duration(duration)
jt.points = [jtp]
#jt.header.stamp = rospy.Time.now()
jt.header.stamp = rospy.Time.from_sec(rospy.Time.now().to_sec() + 0.3)
self.controller_pub.publish(jt)
print 'goto_joint_positions', self.vel_limits
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name,
duration)
self.pr2.start_thread(JustWaitThread(duration))
def goto_joint_positions(self, positions_goal):
"""
This method is eventually called by the Cartesian controller (for servoing)
"""
positions_cur = self.get_joint_positions()
assert len(positions_goal) == len(positions_cur)
duration = norm(
(r_[positions_goal] - r_[positions_cur]) / self.vel_limits,
ord=inf)
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = positions_goal
############################
if ADD_NOISE:
jtp.positions += self.joint_noise
############################
jtp.velocities = zeros(len(positions_goal))
jtp.time_from_start = rospy.Duration(duration)
jt.points = [jtp]
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name,
duration)
self.pr2.start_thread(JustWaitThread(duration))
def goto_joint_positions(self, positions_goal, unwrap = False):
positions_cur = self.get_joint_positions()
assert len(positions_goal) == len(positions_cur)
duration = norm((r_[positions_goal] - r_[positions_cur])/self.vel_limits, ord=inf)
if unwrap:
positions_goal = positions_goal.copy()
for i in [2,4,6]:
positions_goal[i] = np.unwrap([positions_cur[i], positions_goal[i]])[1]
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = positions_goal
jtp.velocities = zeros(len(positions_goal))
jtp.time_from_start = rospy.Duration(duration)
jt.points = [jtp]
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name, duration)
self.pr2.start_thread(JustWaitThread(duration))
def to_joint_trajectory_msg(self):
"""Create trajectory msg for the publisher.
:returns
a ROS msg for the joint trajectory
"""
joint_trajectory_msg = trajectory_msg.JointTrajectory()
joint_trajectory_msg.joint_names = [joint.name for joint in self.joints]
timestamps = self.get_unique_timestamps()
for timestamp in timestamps:
joint_trajectory_point = trajectory_msg.JointTrajectoryPoint()
joint_trajectory_point.time_from_start = rospy.Duration.from_sec(timestamp)
for joint in self.joints:
interpolated_setpoint = joint.get_interpolated_setpoint(timestamp)
if interpolated_setpoint.time != timestamp:
rospy.logwarn('Time mismatch in joint {jn} at timestamp {ts}, '
'got time {ti}'.format(jn=joint.name, ts=timestamp, ti=interpolated_setpoint.time))
joint_trajectory_point.positions.append(interpolated_setpoint.position)
joint_trajectory_point.velocities.append(interpolated_setpoint.velocity)
joint_trajectory_msg.points.append(joint_trajectory_point)
return joint_trajectory_msg
def construct_points(self, posl, tstep):
points = [tm.JointTrajectoryPoint() for i in range(len(posl))]
for i in range(len(posl)):
points[i].positions = posl[i]
points[i].velocities = [0 for j in range(7)]
for i in range(len(posl)):
points[i].time_from_start = rospy.Duration(i*tstep)
return points
def publish_joint_traj(self, pub, joint_traj):
joint_traj_msg = tm.JointTrajectory()
joint_traj_msg.header.stamp = rospy.Time.now()
joint_traj_msg.points = [
tm.JointTrajectoryPoint(positions=joints) for joints in joint_traj
]
pub.publish(joint_traj_msg)
def apply(hsr, arg):
tilt, pan = arg[:2]
point = traj_msg.JointTrajectoryPoint()
point.positions = [tilt, pan]
point.velocities = [0] * 2
point.time_from_start = rospy.Time(0.5)
traj = ctrl_msg.FollowJointTrajectoryActionGoal()
traj.goal.trajectory.joint_names = ['head_tilt_joint', 'head_pan_joint']
traj.goal.trajectory.points = [point]
hsr.publishers['head'].publish(traj)
rospy.sleep(2.)
def follow_timed_trajectory(self, times, xyas, frame_id):
jt = tm.JointTrajectory()
jt.header.frame_id = frame_id
n = len(xyas)
assert len(times) == n
for i in xrange(n):
jtp = tm.JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(times[i])
jtp.positions = xyas[i]
jt.points.append(jtp)
self.traj_pub.publish(jt)
def command_pan_tilt_vel(self, pan_vel, tilt_vel, dt=0.2):
pan, tilt = self.get_joint_positions()
jt = tm.JointTrajectory()
jt.header.stamp = rospy.Time.now() + rospy.Duration(0.01)
jt.joint_names = ["head_pan_joint", "head_tilt_joint"]
jtp = tm.JointTrajectoryPoint(
positions=[pan + pan_vel * dt, tilt + tilt_vel * dt],
velocities=[pan_vel, tilt_vel],
time_from_start=rospy.Duration(dt))
jt.points = [jtp]
self.controller_pub.publish(jt)
self.pr2.start_thread(JustWaitThread(dt))
def follow_timed_trajectory(self, times, angs):
times_up = np.arange(0, times[-1] + 1e-4, .1)
angs_up = np.interp(times_up, times, angs)
jt = tm.JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = ["%s_gripper_joint" % self.lr]
for (t, a) in zip(times, angs):
jtp = tm.JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(t)
jtp.positions = [a]
jt.points.append(jtp)
self.controller_pub.publish(jt)
def scan_motor(angle1, angle2, N, t):
global pub, tilt, pub_flag, Flag_data
motor = rospy.wait_for_message('/dynamixel_workbench/joint_states',
JointState)
cur_pos = motor.position
e = angle1 - cur_pos[0]
t_step = t / N
angles = np.linspace(cur_pos, angle2 + np.abs(e), int(N))
imu = rospy.wait_for_message('/um7', Imu)
q = imu.orientation
pitch_m = q2pitch(q)
if flag_print:
print("Scanning")
print("imu: %.4f" % (pitch_m * 180.0 / np.pi))
print("motor: %.4f" % (cur_pos[0] * 180 / np.pi))
print("---------------------------")
t_i = T.time()
for i in angles:
tilt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = [i]
jtp.velocities = [0]
jtp.time_from_start = rospy.Duration(t_step) # sec - relative to speed
tilt.points = [jtp]
pub.publish(tilt)
T.sleep(5 - t_step)
Flag_data = True
pub_flag.publish(Flag_data)
T.sleep(0.5)
Flag_data = False
pub_flag.publish(Flag_data)
imu = rospy.wait_for_message('/um7', Imu)
q = imu.orientation
pitch_m = q2pitch(q)
motor = rospy.wait_for_message('/dynamixel_workbench/joint_states',
JointState)
cur_pos = motor.position
if flag_print:
print("imu: %.4f" % (pitch_m * 180.0 / np.pi))
print("motor: %.4f" % (cur_pos[0] * 180 / np.pi))
print("Scan time: %.2f seg" % (T.time() - t_i))
print("---------------------------")
def createGripperPositionCommand(newPosition):
msg = tm.JointTrajectory()
point = tm.JointTrajectoryPoint()
point.positions = [newPosition, newPosition]
point.velocities = ones(2)
point.accelerations = zeros(2)
point.time_from_start = rospy.Duration(0.5)
msg.points = [point]
msg.joint_names = ["gripper_finger_joint_l", "gripper_finger_joint_r"]
msg.header.frame_id = "gripper_finger_joint_l"
msg.header.stamp = rospy.Time.now()
return msg
def move_motor(angle):
global pub, tilt
imu = rospy.wait_for_message('/um7', Imu)
q = imu.orientation
pitch_m = q2pitch(q)
motor = rospy.wait_for_message('/dynamixel_workbench/joint_states',
JointState)
cur_pos = motor.position
up = 0.0
ui = 0.0
while np.abs(angle - pitch_m) > 0.02: # error of +/- 2°
e = angle - pitch_m
up = 0.75 * e
ui = 0.4 * e + ui
u = up + ui + cur_pos[0]
tilt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = [u]
jtp.velocities = np.zeros(len(jtp.positions))
jtp.time_from_start = rospy.Duration(2) # sec - relative to speed
tilt.points = [jtp]
pub.publish(tilt)
T.sleep(2.1)
imu = rospy.wait_for_message('/um7', Imu)
q = imu.orientation
pitch_m = q2pitch(q)
motor = rospy.wait_for_message('/dynamixel_workbench/joint_states',
JointState)
cur_pos = motor.position
if flag_print:
print("IMU_measurement: %.4f deg" % (pitch_m * 180.0 / np.pi))
print("Motor_measurement: %.4f deg" % (cur_pos[0] * 180 / np.pi))
print("Error: %.4f deg" %
((pitch_m - cur_pos[0]) * 180 / np.pi))
print("---------------------------")
return pitch_m
def createArmPositionCommand(newPositions):
msg = tm.JointTrajectory()
point = tm.JointTrajectoryPoint()
point.positions = newPositions
point.velocities = zeros(len(newPositions))
point.accelerations = zeros(len(newPositions))
point.time_from_start = rospy.Duration(0.5)
msg.points = [point]
jointNames = []
for i in range(5):
jointNames.append("arm_joint_" + str(i + 1))
msg.joint_names = jointNames
msg.header.frame_id = "arm_link_0"
msg.header.stamp = rospy.Time.now()
return msg
def follow_timed_joint_trajectory(self, positions, velocities, times):
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
for (position, velocity, time) in zip(positions, velocities, times):
jtp = tm.JointTrajectoryPoint()
jtp.positions = position
jtp.velocities = velocity
jtp.time_from_start = rospy.Duration(time)
jt.points.append(jtp)
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name, times[-1])
self.pr2.start_thread(JustWaitThread(times[-1]))
def apply(hsr, arg):
arm_lift, arm_flex, arm_roll, wrist_flex, wrist_roll = arg[:5]
point = traj_msg.JointTrajectoryPoint()
point.positions = [arm_lift, arm_flex, arm_roll, wrist_flex, wrist_roll]
point.velocities = [0] * 5
point.time_from_start = rospy.Time(0.5)
traj = ctrl_msg.FollowJointTrajectoryActionGoal()
traj.goal.trajectory.joint_names = ['arm_lift_joint', 'arm_flex_joint', 'arm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']
traj.goal.trajectory.points = [point]
hsr.publishers['arm'].publish(traj)
rospy.sleep(1.)
while True:
error = hsr.subscribers['arm_pose'].callback.data['error']
if all(abs(x) <= 1e-2 for x in error):
break
rospy.sleep(0.1)
def set_angle(self, a, max_effort=default_max_effort):
a_cur = self.get_angle()
duration = abs((a - a_cur) / self.vel_limits[0])
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = [a]
jtp.velocities = [0]
jtp.time_from_start = rospy.Duration(duration)
jt.points = [jtp]
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name,
duration)
self.pr2.start_thread(JustWaitThread(duration))
def send_joint_positions(self, joint_positions, duration):
"""
Publishes joint_positions to be executed
in time duration
"""
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = joint_positions
jtp.time_from_start = duration
################################
if ADD_NOISE:
jtp.positions += self.joint_noise
################################
jt.points = [jtp]
self.controller_pub.publish(jt)
def angle_change(self,data):
self.angle = data.data
if self.angle<-2.094:
self.angle = -2.094
elif self.angle > 0:
self.angle = 0.0
tilt = tm.JointTrajectory()
tilt.header.frame_id = "joint_trajectory"
tilt.header.stamp = rospy.Time.now()
tilt.joint_names = ["tilt"]
jtp = tm.JointTrajectoryPoint()
jtp.positions = [self.angle]
jtp.velocities = [0.5]
jtp.effort = [0.3]
jtp.time_from_start = rospy.Duration(2)
tilt.points = [jtp]
self.pub.publish(tilt)
def _create_trajectory(self, pos_mat, times, vel_mat=None):
#Make JointTrajectoryPoints
points = [tm.JointTrajectoryPoint() for i in range(pos_mat.shape[1])]
for i in range(pos_mat.shape[1]):
points[i].positions = pos_mat[:, i].A1.tolist()
points[i].accelerations = self.zeros
if vel_mat == None:
points[i].velocities = self.zeros
else:
points[i].velocities = vel_mat[:, i].A1.tolist()
for i in range(pos_mat.shape[1]):
points[i].time_from_start = rospy.Duration(times[i])
#Create JointTrajectory
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.points = points
jt.header.stamp = rospy.get_rostime()
return jt
def execute_joint_trajectory(self, joint_traj, block=True, speed=None):
"""
:param joint_traj: list of joint waypoints
:param block: if True, waits until trajectory is completed
:param speed: execution speed in meters/sec
:return if not blocking, return expected execution time
"""
for joints in joint_traj:
assert joints is not None
assert len(joints) == len(self.current_joints)
speed = float(speed) if speed is not None else self.default_speed
goal = pcm.JointTrajectoryGoal()
#joint_trajectory = tm.JointTrajectory()
goal.trajectory.joint_names = self.joint_names
goal.trajectory.header.stamp = rospy.Time.now()
curr_joints = self.current_joints
time_from_start = 0.
for next_joints in joint_traj:
next_position = self.fk(next_joints).position
curr_position = self.fk(curr_joints).position
duration = ((next_position - curr_position).norm)/speed
time_from_start += duration
waypoint = tm.JointTrajectoryPoint()
waypoint.positions = next_joints
waypoint.time_from_start = rospy.Duration(time_from_start)
goal.trajectory.points.append(waypoint)
curr_joints = next_joints
self.joint_command_client.send_goal(goal)
#self.joint_command_pub.publish(goal.trajectory)
if block:
rospy.sleep(time_from_start)
else:
return time_from_start
def go2top():
global pub, tilt
print("=====================================================")
print("Scan node is beginning")
print("=====================================================")
angle = np.pi / (-2.0)
time_to_move = 3
for i in [0, 0, angle]:
tilt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = [i]
jtp.velocities = np.zeros(1)
jtp.time_from_start = rospy.Duration(
time_to_move) # sec - relative to speed
tilt.points = [jtp]
pub.publish(tilt)
T.sleep(time_to_move + 0.1)
def apply(hsr, arg):
x, y, theta, v, omega = arg[:5]
x0, y0, theta0 = hsr.subscribers['base_pose'].callback.data['actual']
dx = ((x - x0) ** 2 + (y - y0) ** 2) ** .5
dtheta = abs(theta - theta0)
t = max(dx / v, dtheta / omega)
acceleration = 0.05
point = traj_msg.JointTrajectoryPoint()
point.positions = [x, y, theta]
point.accelerations = [acceleration] * 3
point.effort = [1]
point.time_from_start = rospy.Time(t)
traj = ctrl_msg.FollowJointTrajectoryActionGoal()
traj.goal.trajectory.joint_names = ['odom_x', 'odom_y', 'odom_t']
traj.goal.trajectory.points = [point]
hsr.publishers['base'].publish(traj)
rospy.sleep(1.)
while True:
error = hsr.subscribers['base_pose'].callback.data['error']
if all(abs(x) <= 1e-6 for x in error):
break
rospy.sleep(0.1)
def get_joint_trajectory_msg(self) -> trajectory_msg.JointTrajectory:
"""Return a joint_trajectory_msg containing the interpolated
trajectories for each joint
:returns: A joint_trajectory_msg
:rtype: joint_trajectory_msg
"""
# Update pose:
pose_list = [joint.position for joint in self.starting_position.values()]
self.pose = Pose(pose_list)
self._solve_middle_setpoint()
self._solve_desired_setpoint()
self._get_extra_ankle_setpoint()
# Create joint_trajectory_msg
self._to_joint_trajectory_class()
joint_trajectory_msg = trajectory_msg.JointTrajectory()
joint_trajectory_msg.joint_names = self.joint_names
timestamps = np.linspace(self.time[0], self.time[-1], INTERPOLATION_POINTS)
for timestamp in timestamps:
joint_trajecory_point = trajectory_msg.JointTrajectoryPoint()
joint_trajecory_point.time_from_start = Duration(timestamp).to_msg()
for joint_index, joint_trajectory in enumerate(self.joint_trajectory_list):
interpolated_setpoint = joint_trajectory.get_interpolated_setpoint(
timestamp
)
joint_trajecory_point.positions.append(interpolated_setpoint.position)
joint_trajecory_point.velocities.append(interpolated_setpoint.velocity)
self._check_joint_limits(joint_index, joint_trajecory_point)
joint_trajectory_msg.points.append(joint_trajecory_point)
return joint_trajectory_msg
def follow_timed_joint_trajectory(self, positions, velocities, times):
"""
This method is eventually called using trajopt output
"""
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
for (position, velocity, time) in zip(positions, velocities, times):
jtp = tm.JointTrajectoryPoint()
#################################
if ADD_NOISE:
position = position + self.joint_noise
#################################
jtp.positions = position
jtp.velocities = velocity
jtp.time_from_start = rospy.Duration(time)
jt.points.append(jtp)
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name,
times[-1])
self.pr2.start_thread(JustWaitThread(times[-1]))
def callback(getreq):
assert isinstance(getreq, ms.GetMotionPlanRequest)
req = getreq.motion_plan_request
getresp = ms.GetMotionPlanResponse()
resp = getresp.motion_plan_response
manip = GROUPMAP[req.group_name]
update_rave_from_ros(robot, req.start_state.joint_state.position, req.start_state.joint_state.name)
base_pose = req.start_state.multi_dof_joint_state.poses[0]
base_q = base_pose.orientation
base_p = base_pose.position
t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z])
robot.SetTransform(t)
start_joints = robot.GetDOFValues(robot.GetManipulator(manip).GetArmIndices())
n_steps = 9
coll_coeff = 10
dist_pen = .04
inds = robot.GetManipulator(manip).GetArmJoints()
alljoints = robot.GetJoints()
names = arm_joint_names = [alljoints[i].GetName() for i in inds]
for goal_cnt in req.goal_constraints:
if len(goal_cnt.joint_constraints) > 0:
assert len(goal_cnt.joint_constraints)==7
end_joints = np.zeros(7)
for i in xrange(7):
jc = goal_cnt.joint_constraints[i]
dof_idx = arm_joint_names.index(jc.joint_name)
end_joints[dof_idx] = jc.position
waypoint_step = (n_steps - 1)// 2
joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints))/2]
if args.multi_init:
leftright = req.group_name.split("_")[0]
joint_waypoints.extend(get_postures(leftright))
success = False
for waypoint in joint_waypoints:
robot.SetDOFValues(start_joints, inds)
d = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : manip,
"start_fixed" : True
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [1]}
},
{
"type" : "continuous_collision",
"params" : {"coeffs" : [coll_coeff],"dist_pen" : [dist_pen]}
}
],
"constraints" : [],
"init_info" : {
"type" : "given_traj"
}
}
d["constraints"].append({
"type" : "joint",
"name" : "joint",
"params" : {
"vals" : end_joints.tolist()
}
})
if args.multi_init:
inittraj = np.empty((n_steps, 7))
inittraj[:waypoint_step+1] = mu.linspace2d(start_joints, waypoint, waypoint_step+1)
inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step)
else:
inittraj = mu.linspace2d(start_joints, end_joints, n_steps)
print "this",inittraj
print "other",mu.linspace2d(start_joints, end_joints, n_steps)
d["init_info"]["data"] = [row.tolist() for row in inittraj]
if len(goal_cnt.position_constraints) > 0:
raise NotImplementedError
s = json.dumps(d)
t_start = time()
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
planning_duration_seconds = time() - t_start
traj = result.GetTraj()
if check_traj(traj, robot.GetManipulator(manip)):
print "aw, there's a collision"
else:
print "no collisions"
success = True
break
if not success:
resp.error_code.val = FAILURE
return resp
resp.trajectory.joint_trajectory.joint_names = names
for row in traj:
jtp = tm.JointTrajectoryPoint()
jtp.positions = row.tolist()
jtp.time_from_start = rospy.Duration(0)
resp.trajectory.joint_trajectory.points.append(jtp)
mdjt = resp.trajectory.multi_dof_joint_trajectory
mdjt.joint_names = ['world_joint']
mdjt.frame_ids = ['odom_combined']
mdjt.child_frame_ids = ['base_footprint']
mdjt.header = req.start_state.joint_state.header
pose = gm.Pose()
pose.orientation.w = 1
for _ in xrange(len(resp.trajectory.joint_trajectory.points)):
jtp = mm.MultiDOFJointTrajectoryPoint()
jtp.poses.append(pose)
#for i in xrange(len(mdjs.joint_names)):
#if mdjs.joint_names[i] == "world_joint":
#world_joint_pose = mdjs.poses[i]
#world_joint_frame_id = mdjs.frame_ids[i]
#world_joint_child_frame_id = mdjs.child_frame_ids[i]
#print "world_joint_frame_id", world_joint_frame_id
#print "world_joint_child_frame_id", world_joint_child_frame_id
#mdjt = resp.trajectory.multi_dof_joint_trajectory
#mdjt.header.frame_id = req.start_state.joint_state.header.frame_id
#mdjt.joint_names.append("world_joint")
resp.error_code.val = SUCCESS
resp.planning_time = rospy.Duration(planning_duration_seconds)
resp.trajectory.joint_trajectory.header = req.start_state.joint_state.header
resp.group_name = req.group_name
resp.trajectory_start.joint_state = req.start_state.joint_state
#resp.trajectory.multi_dof_joint_trajectory
#resp.trajectory.multi_dof_joint_trajectory.header = req.start_state.joint_state.header
getresp.motion_plan_response = resp
return getresp
def _check_time_validity(self, value):
r, g, b = 0, 0, 0
palette = QPalette(QColor(r, g, b, 255))
palette.setColor(QPalette.Text, QColor(r, g, b, 255))
self.time_box.setPalette(palette)
jang_list = self.get_joint_angs_list()
left_list, right_list = split_joints_list(jang_list)
#arm = tu.selected_radio_button(self.arm_radio_buttons).lower()
arm = self.get_arm_radio()
if arm == 'left':
#idx = 0
pref = 'l_'
left_arm = True
ang_list = left_list
else:
#idx = 1
pref = 'r_'
left_arm = False
ang_list = right_list
if len(ang_list) == 0:
return
if self.list_manager.get_selected_idx() == None:
ref = ang_list[-1]
else:
sidx = self.list_manager.get_selected_idx()
ang_list_idx = None
for idx, d in enumerate(ang_list):
if d == jang_list[sidx]:
ang_list_idx = idx
if ang_list_idx == None:
return
pidx = ang_list_idx - 1
if pidx < 0:
return
else:
ref = ang_list[pidx]
start_point = tm.JointTrajectoryPoint()
start_point.velocities = [0.] * len(p2u.JOINT_NAME_FIELDS)
for name in p2u.JOINT_NAME_FIELDS:
exec('box = self.%s' % name)
start_point.positions.append(np.radians(box.value()))
end_point = tm.JointTrajectoryPoint()
end_point.velocities = [0.] * len(p2u.JOINT_NAME_FIELDS)
end_point.positions = ref['angs']
min_time = self.min_time_service(start_point, end_point, left_arm).time
curr_time = self.time_box.value()
#print 'min_time', min_time
for multiplier, color in [[1., [255, 0, 0]], [2., [255, 153, 0]]]:
if curr_time <= (min_time * multiplier):
r, g, b = color
palette = QPalette(QColor(r, g, b, 255))
palette.setColor(QPalette.Text, QColor(r, g, b, 255))
self.time_box.setPalette(palette)
return
