def runTest(self): rpy1 = np.deg2rad([10, -30, 90]) R1 = rox.rpy2R(rpy1) R1_t=np.array([[-0.0000000, -0.9848077, 0.1736482], \ [0.8660254, -0.0868241, -0.4924039], \ [0.5000000, 0.1503837, 0.8528686 ]]) np.testing.assert_allclose(R1, R1_t, atol=1e-6) rpy2 = rox.R2rpy(R1) np.testing.assert_allclose(rpy1, rpy2, atol=1e-6) #Check singularity rpy3 = np.deg2rad([10, 90, -30]) R3 = rox.rpy2R(rpy3) self.assertRaises(Exception, lambda: rox.R2rpy(R3))
def _xyz_rpy_to_rox_transform(self, xyz, rpy, parent_frame_id=None, child_frame_id=None): p = xyz R = rox.rpy2R(rpy) return rox.Transform(R, p, parent_frame_id, child_frame_id)
def plan_robot_motion(self, dxf_points, sines): self.dxf_points = dxf_points self.sines = sines waypoints = [] message = rospy.wait_for_message("/robot_status", RobotStatus) joint_state = rospy.wait_for_message("/joint_states", JointState) while (message.in_motion.val == 1): message = rospy.wait_for_message("/robot_status", RobotStatus) #self.move_group.clear_pose_targets() moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state self.move_group.set_start_state(moveit_robot_state) #wpose = self.move_group.get_current_pose().pose #waypoints.append(wpose) #state = self.robot.get_current_state() #self.move_group.set_start_state(state) for i in range(len(dxf_points)): pose_goal = Pose() rpy = [math.pi, 0.0, sines[i]] print(rpy) R_result = rox.rpy2R(rpy) quat = rox.R2q(R_result) print(quat) pose_goal.orientation.w = 0.0 pose_goal.orientation.x = quat[1] pose_goal.orientation.y = quat[2] pose_goal.orientation.z = 0.0 #20- sets setpoint in middle of dxf file for robot y axis #middle of dxf y axis is 0, so no centering needed here x_val = (20 - dxf_points[i][0]) * 0.0254 y_val = dxf_points[i][1] * 0.0254 pose_goal.position.x = 0.8 + y_val pose_goal.position.y = x_val pose_goal.position.z = 0.3 print(pose_goal) waypoints.append(pose_goal) replan_value = 0 error_code = None (plan, fraction) = self.move_group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold while (replan_value < 3 and fraction < 1.0): print(fraction) (plan, fraction) = self.move_group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold replan_value += 1 print("WARNING Portion of plan failed, replanning") return plan, fraction, waypoints
def test_geometry_util_array_types(): node = RR.RobotRaconteurNode() node.SetLogLevelFromString("DEBUG") node.Init() try: RRC.RegisterStdRobDefServiceTypes(node) geom_util = GeometryUtil(node) _do_array_test(geom_util.xy_to_vector2, geom_util.vector2_to_xy, (2, ), "Vector2", node) _do_array_test(geom_util.xyz_to_vector3, geom_util.vector3_to_xyz, (3, ), "Vector3", node) _do_array_test(geom_util.abgxyz_to_vector6, geom_util.vector6_to_abgxyz, (6, ), "Vector6", node) _do_array_test(geom_util.xy_to_point2d, geom_util.point2d_to_xy, (2, ), "Point2D", node) _do_array_test(geom_util.xyz_to_point, geom_util.point_to_xyz, (3, ), "Point", node) _do_array_test(geom_util.wh_to_size2d, geom_util.size2d_to_wh, (2, ), "Size2D", node) _do_array_test(geom_util.whd_to_size, geom_util.size_to_whd, (3, ), "Size", node) _do_array_test(geom_util.q_to_quaternion, geom_util.quaternion_to_q, (3, ), "Quaternion", node, lambda a: rox.R2q(rox.rpy2R(a))) _do_array_test(geom_util.R_to_quaternion, geom_util.quaternion_to_R, (3, ), "Quaternion", node, lambda a: rox.rpy2R(a)) _do_array_test(geom_util.rpy_to_quaternion, geom_util.quaternion_to_rpy, (3, ), "Quaternion", node) _do_array_test(geom_util.array_to_spatial_velocity, geom_util.spatial_velocity_to_array, (6, ), "SpatialVelocity", node) _do_array_test(geom_util.array_to_spatial_acceleration, geom_util.spatial_acceleration_to_array, (6, ), "SpatialAcceleration", node) _do_array_test(geom_util.array_to_wrench, geom_util.wrench_to_array, (6, ), "Wrench", node) finally: node.Shutdown()
def _do_transform_test(to_rr, from_rr, rr_type, node): rox_transform = rox.Transform(rox.rpy2R(np.random.rand(3)), np.random.rand(3)) rr_val = to_rr(rox_transform) rr_msg = PackMessageElement(rr_val, f"com.robotraconteur.geometry.{rr_type}", node=node) rr_msg.UpdateData() rr_val2 = UnpackMessageElement(rr_msg, node=node) rox_transform2 = from_rr(rr_val2) np.testing.assert_almost_equal(rox_transform.R, rox_transform2.R) np.testing.assert_almost_equal(rox_transform.p, rox_transform2.p)
def move_to_seam_start(self, motion_point, sine): pose_goal = Pose() rpy = [math.pi, 0.0, sine - math.pi / 2] print(rpy) R_result = rox.rpy2R(rpy) quat = rox.R2q(R_result) print(quat) pose_goal.orientation.w = 0.0 pose_goal.orientation.x = quat[1] pose_goal.orientation.y = quat[2] pose_goal.orientation.z = 0.0 #20- sets setpoint in middle of dxf file for robot y axis #middle of dxf y axis is 0, so no centering needed here x_val = (20 - motion_point[0]) * 0.0254 y_val = motion_point[1] * 0.0254 pose_goal.position.x = 0.8 + y_val pose_goal.position.y = x_val pose_goal.position.z = 0.3 pose_goal.position.x += 0.1 #self.robot.set_start_state_to_current_state() self.move_group.set_pose_target(pose_goal) result = self.move_group.go(wait=True)
def pbvs_jacobian(self): self.controller_commander.set_controller_mode( self.controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [], []) tvec_err = [100, 100, 100] rvec_err = [100, 100, 100] self.FTdata_0 = self.FTdata error_transform = rox.Transform(rox.rpy2R([2, 2, 2]), np.array([100, 100, 100])) FT_data_ori = [] FT_data_biased = [] err_data_p = [] err_data_rpy = [] joint_data = [] time_data = [] #TODO: should listen to stage_3_tol_r not 1 degree print self.desired_camera2_transform #R_desired_cam = self.desired_camera2_transform.R.dot(self.desired_transform.R) #R_desired_cam = R_desired_cam.dot(self.desired_camera2_transform.R.transpose()) #t_desired_cam = -self.desired_camera2_transform.R.dot(self.desired_transform.p) while (error_transform.p[2] > 0.01 or np.linalg.norm( [error_transform.p[0], error_transform.p[1]]) > self.stage3_tol_p or np.linalg.norm(rox.R2rpy(error_transform.R)) > np.deg2rad(1)): img = self.receive_image() fixed_marker_transform, payload_marker_transform, error_transform = self.compute_error_transform( img) #print self.desired_transform.R.T, -fixed_marker_transform.R.dot(self.desired_transform.p) R_desired_cam = fixed_marker_transform.R.dot( self.desired_transform.R) R_desired_cam = R_desired_cam.dot( fixed_marker_transform.R.transpose()) t_desired_cam = -fixed_marker_transform.R.dot( self.desired_transform.p) # Compute error directly in the camera frame observed_R_difference = np.dot( payload_marker_transform.R, fixed_marker_transform.R.transpose()) k, theta = rox.R2rot( np.dot(observed_R_difference, R_desired_cam.transpose()) ) #np.array(rox.R2rpy(rvec_err1)) rvec_err1 = k * theta observed_tvec_difference = fixed_marker_transform.p - payload_marker_transform.p tvec_err1 = -fixed_marker_transform.R.dot( self.desired_transform.p) - observed_tvec_difference #print 'SPOT: ',tvec_err1, rvec_err1 # Map error to the robot spatial velocity world_to_camera_tf = self.tf_listener.lookupTransform( "world", "gripper_camera_2", rospy.Time(0)) camera_to_link6_tf = self.tf_listener.lookupTransform( "gripper_camera_2", "link_6", rospy.Time(0)) t21 = -np.dot( rox.hat( np.dot(world_to_camera_tf.R, (camera_to_link6_tf.p - payload_marker_transform.p.reshape((1, 3))).T)), world_to_camera_tf.R) #np.zeros((3,3))# # v = R_oc(vc)c + R_oc(omeega_c)_c x (r_pe)_o = R_oc(vc)c - (r_pe)_o x R_oc(omeega_c)_c tvec_err = t21.dot(rvec_err1).reshape( (3, )) + world_to_camera_tf.R.dot(tvec_err1).reshape((3, )) # omeega = R_oc(omeega_c)_c rvec_err = world_to_camera_tf.R.dot(rvec_err1).reshape((3, )) tvec_err = np.clip(tvec_err, -0.2, 0.2) rvec_err = np.clip(rvec_err, -np.deg2rad(5), np.deg2rad(5)) if tvec_err[2] < 0.03: rospy.loginfo("Only Compliance Control") tvec_err[2] = 0 rot_err = rox.R2rpy(error_transform.R) rospy.loginfo("tvec difference: %f, %f, %f", error_transform.p[0], error_transform.p[1], error_transform.p[2]) rospy.loginfo("rvec difference: %f, %f, %f", rot_err[0], rot_err[1], rot_err[2]) dx = -np.concatenate((rvec_err, tvec_err)) * self.K_pbvs print np.linalg.norm([error_transform.p[0], error_transform.p[1]]) print np.linalg.norm(rox.R2rpy(error_transform.R)) # Compliance Force Control if (not self.ft_flag): raise Exception("havent reached FT callback") # Compute the exteranl force FTread = self.FTdata - self.FTdata_0 # (F)-(F0) print '================ FT1 =============:', FTread print '================ FT2 =============:', self.FTdata if FTread[-1] > (self.F_d_set1 + 50): F_d = self.F_d_set1 else: F_d = self.F_d_set2 if (self.FTdata == 0).all(): rospy.loginfo("FT data overflow") dx[-1] += self.K_pbvs * 0.004 else: tx_correct = 0 if abs(self.FTdata[0]) > 80: tx_correct = 0.0002 * (abs(self.FTdata[0]) - 80) Vz = self.Kc * (F_d - FTread[-1]) + tx_correct dx[-1] = dx[-1] + Vz print "dx:", dx current_joint_angles = self.controller_commander.get_current_joint_values( ) joints_vel = QP_abbirb6640( np.array(current_joint_angles).reshape(6, 1), np.array(dx)) goal = self.trapezoid_gen( np.array(current_joint_angles) + joints_vel.dot(1), np.array(current_joint_angles), 0.008, 0.008, 0.015) #acc,dcc,vmax) print "joints_vel:", joints_vel self.client.wait_for_server() self.client.send_goal(goal) self.client.wait_for_result() res = self.client.get_result() if (res.error_code != 0): raise Exception("Trajectory execution returned error") FT_data_ori.append(self.FTdata) FT_data_biased.append(FTread) err_data_p.append(error_transform.p) err_data_rpy.append(rot_err) joint_data.append(current_joint_angles) time_data.append(time.time()) filename_pose = "/home/rpi-cats/Desktop/YC/Data/Panel2_Placement_In_Nest_Pose_" + str( time.time()) + ".mat" scipy.io.savemat(filename_pose, mdict={ 'FT_data_ori': FT_data_ori, 'FT_data_biased': FT_data_biased, 'err_data_p': err_data_p, 'err_data_rpy': err_data_rpy, 'joint_data': joint_data, 'time_data': time_data }) rospy.loginfo("End ====================")
def urdf_to_payload(xml_str): urdf_robot = URDF.from_xml_string(xml_str) urdf_et = ET.fromstring(xml_str) base_links = [ u for u in urdf_robot.child_map.keys() if u not in urdf_robot.parent_map ] assert len(base_links) == 1, "Multiple payloads detected, invalid URDF." base_link = base_links[0] assert base_link not in urdf_robot.link_map, "Invalid initial_pose parent link in payload URDF" assert len(urdf_robot.child_map[base_link] ) == 1, "Invalid initial_pose in payload URDF" (initial_pose_joint_name, payload_link_name) = urdf_robot.child_map[base_link][0] assert initial_pose_joint_name.endswith( '_initial_pose'), "Invalid initial_pose parent link in payload URDF" #assert all([j.type == "fixed" for _, j in urdf_robot.joint_map.items()]), "All joints must be fixed type in payload URDF" assert all([urdf_robot.parent_map[l][1] == payload_link_name for l in urdf_robot.link_map if l != payload_link_name]), \ "All links must have payload link as parent" payload_link = urdf_robot.link_map[payload_link_name] initial_pose_joint = urdf_robot.joint_map[initial_pose_joint_name] payload_msg = Payload() payload_msg.name = payload_link_name #Load in initial pose payload_msg.header.frame_id = initial_pose_joint.parent payload_msg.pose = _origin_to_pose(initial_pose_joint.origin) #Load in inertia if payload_link.inertial is not None: m = Inertia() m.m = payload_link.inertial.mass m.ixx = payload_link.inertial.inertia.ixx m.ixy = payload_link.inertial.inertia.ixy m.ixz = payload_link.inertial.inertia.ixz m.iyy = payload_link.inertial.inertia.iyy m.iyz = payload_link.inertial.inertia.iyz m.izz = payload_link.inertial.inertia.izz if payload_link.inertial.origin is not None: if payload_link.inertial.origin.xyz is not None: m.com.x = payload_link.inertial.origin.xyz[0] m.com.y = payload_link.inertial.origin.xyz[1] m.com.z = payload_link.inertial.origin.xyz[2] if payload_link.inertial.origin.rpy is not None: R = np.matrix( rox.rpy2R(np.array(payload_link.inertial.origin.rpy))) I = np.matrix([[m.ixx, m.ixy, m.ixz], [m.ixy, m.iyy, m.iyz], [m.ixz, m.iyz, m.izz]]) I2 = np.dot(np.transpose(R), I).dot(R) m.ixx = I2[0, 0] m.ixy = I2[0, 1] m.ixz = I2[0, 2] m.ixy = I2[1, 0] m.iyy = I2[1, 1] m.iyz = I2[1, 2] m.izz = I2[2, 2] payload_msg.inertia = m #Load in gripper targets for _, l in urdf_robot.link_map.items(): m = re.match(r'^\w+_gripper_target(?:_(\d+))?$', l.name) if m is None: continue j = urdf_robot.joint_map[urdf_robot.parent_map[l.name][0]] assert j.parent == payload_link_name, "Invalid gripper_target for payload in URDF" pose = _origin_to_pose(j.origin) gripper_target = GripperTarget() gripper_target.header.frame_id = payload_link_name gripper_target.name = l.name gripper_target.pose = pose link_et = urdf_et.find('.//link[@name="' + l.name + '"]') ft_et = link_et.find('.//gripper_ft_threshold') if ft_et is not None: pickup_et = ft_et.find('.//pickup') if pickup_et is not None: ft_val = [float(d) for d in pickup_et.attrib['ft'].split()] assert len(ft_val) == 6, "Invalid pickup ft" gripper_target.pickup_ft_threshold = ft_val place_et = ft_et.find('.//place') if place_et is not None: ft_val = [float(d) for d in place_et.attrib['ft'].split()] assert len(ft_val) == 6, "Invalid pickup ft" gripper_target.place_ft_threshold = ft_val payload_msg.gripper_targets.append(gripper_target) #Load in markers for _, l in urdf_robot.link_map.items(): m = re.match(r'^\w+_marker(?:_(\d+))?$', l.name) if m is None: continue j = urdf_robot.joint_map[urdf_robot.parent_map[l.name][0]] assert j.parent == payload_link_name, "Invalid marker for payload in URDF" pose = _origin_to_pose(j.origin) aruco_marker = next((x for x in payload_msg.markers if x.name == l), None) aruco_marker = ArucoGridboardWithPose() aruco_marker.header.frame_id = payload_link_name aruco_marker.name = l.name aruco_marker.pose = pose link_et = urdf_et.find('.//link[@name="' + l.name + '"]') marker_et = link_et.find('.//aruco_marker') if marker_et is not None: gridboard_et = marker_et.find('.//gridboard') if gridboard_et is not None: a = gridboard_et.attrib aruco_marker.marker.markersX = int(a['markersX']) aruco_marker.marker.markersY = int(a['markersY']) aruco_marker.marker.markerLength = float(a['markerLength']) aruco_marker.marker.markerSpacing = float(a['markerSpacing']) aruco_marker.marker.dictionary = a['dictionary'] aruco_marker.marker.firstMarker = int(a['firstMarker']) payload_msg.markers.append(aruco_marker) #Load in meshes payload_geometry_chain = [(None, payload_link_name)] if payload_link_name in urdf_robot.child_map: payload_geometry_chain.extend(urdf_robot.child_map[payload_link_name]) for j_name, l_name in payload_geometry_chain: #v=urdf_robot.link_map[l_name].visual j = urdf_robot.joint_map[j_name] if j_name is not None else None link_et = urdf_et.find('.//link[@name="' + l_name + '"]') #Workaround to handle multiple visual elements visual_et_s = link_et.findall('.//visual') for visual_et in visual_et_s: v = Visual.from_xml(visual_et) if j is not None: visual_pose = rox_msg.transform2pose_msg( rox_msg.msg2transform(_origin_to_pose(j.origin)) * rox_msg.msg2transform(_origin_to_pose(v.origin))) else: visual_pose = _origin_to_pose(v.origin) if v.material is None or v.material.color is None: visual_color = ColorRGBA(0.5, 0.5, 0.5, 1) else: visual_color = ColorRGBA(*v.material.color.rgba) if isinstance(v.geometry, Mesh): if v.geometry.scale is None: mesh_scale = Vector3(1, 1, 1) else: mesh_scale = Vector3(*v.geometry.scale) mesh_fname = v.geometry.filename payload_msg.visual_geometry.mesh_poses.append(visual_pose) payload_msg.visual_geometry.mesh_resources.append(mesh_fname) payload_msg.visual_geometry.mesh_scales.append(mesh_scale) payload_msg.visual_geometry.mesh_colors.append(visual_color) elif isinstance(v.geometry, Box): shape = SolidPrimitive() shape.type = SolidPrimitive.BOX shape.dimensions = v.geometry.size payload_msg.visual_geometry.primitives.append(shape) payload_msg.visual_geometry.primitive_poses.append(visual_pose) payload_msg.visual_geometry.primitive_colors.append( visual_color) elif isinstance(v.geometry, Cylinder): shape = SolidPrimitive() shape.type = SolidPrimitive.CYLINDER shape.dimensions = [v.geometry.length, v.geometry.radius] payload_msg.visual_geometry.primitives.append(shape) payload_msg.visual_geometry.primitive_poses.append(visual_pose) payload_msg.visual_geometry.primitive_colors.append( visual_color) elif isinstance(v.geometry, Sphere): shape = SolidPrimitive() shape.type = SolidPrimitive.SPHERE shape.dimensions = [v.geometry.radius] payload_msg.visual_geometry.primitives.append(shape) payload_msg.visual_geometry.primitive_poses.append(visual_pose) payload_msg.visual_geometry.primitive_colors.append( visual_color) else: assert False, "Invalid geometry in URDF" #Workaround to handle multiple collision elements collision_et_s = link_et.findall('.//collision') for collision_et in collision_et_s: v = Collision.from_xml(collision_et) if j is not None: collision_pose = rox_msg.transform2pose_msg( rox_msg.msg2transform(_origin_to_pose(j.origin)) * rox_msg.msg2transform(_origin_to_pose(v.origin))) else: collision_pose = _origin_to_pose(v.origin) if isinstance(v.geometry, Mesh): if v.geometry.scale is None: mesh_scale = Vector3(1, 1, 1) else: mesh_scale = Vector3(*v.geometry.scale) mesh_fname = v.geometry.filename payload_msg.collision_geometry.mesh_poses.append( collision_pose) payload_msg.collision_geometry.mesh_resources.append( mesh_fname) payload_msg.collision_geometry.mesh_scales.append(mesh_scale) elif isinstance(v.geometry, Box): shape = SolidPrimitive() shape.type = SolidPrimitive.BOX shape.dimensions = v.geometry.size payload_msg.collision_geometry.primitives.append(shape) payload_msg.collision_geometry.primitive_poses.append( collision_pose) elif isinstance(v.geometry, Cylinder): shape = SolidPrimitive() shape.type = SolidPrimitive.CYLINDER shape.dimensions = [v.geometry.length, v.geometry.radius] payload_msg.collision_geometry.primitives.append(shape) payload_msg.collision_geometry.primitive_poses.append( collision_pose) elif isinstance(v.geometry, Sphere): shape = SolidPrimitive() shape.type = SolidPrimitive.SPHERE shape.dimensions = [v.geometry.radius] payload_msg.collision_geometry.primitives.append(shape) payload_msg.collision_geometry.primitive_poses.append( collision_pose) else: assert False, "Invalid geometry in URDF" payload_msg.confidence = 0.1 #TODO: read inertial values #Sanity check payload_msg._check_types() return payload_msg
def _origin_to_pose(origin): R = rox.rpy2R(origin.rpy) if origin.rpy is not None else np.eye(3) p = origin.xyz if origin.xyz is not None else np.zeros((3, )) t = rox.Transform(R, p) return rox_msg.transform2pose_msg(t)
def plan_robot_motion(self, dxf_points, sines): self.dxf_points = dxf_points self.sines = sines waypoints = [] message = rospy.wait_for_message("/robot_status", RobotStatus) joint_state = rospy.wait_for_message("/joint_states", JointState) while (message.in_motion.val == 1): message = rospy.wait_for_message("/robot_status", RobotStatus) #self.move_group.clear_pose_targets() print("hello") moveit_robot_state = RobotState() moveit_robot_state.joint_state = joint_state self.move_group.set_start_state(moveit_robot_state) #waypoints.append(self.move_group.get_current_pose().pose) #wpose = self.move_group.get_current_pose().pose #waypoints.append(wpose) #state = self.robot.get_current_state() #self.move_group.set_start_state(state) for i in range(len(dxf_points)): pose_goal = Pose() rpy = [math.pi, 0.0, sines[i] - math.pi / 2] print(rpy) R_result = rox.rpy2R(rpy) quat = rox.R2q(R_result) print(quat) pose_goal.orientation.w = 0.0 pose_goal.orientation.x = quat[1] pose_goal.orientation.y = quat[2] pose_goal.orientation.z = 0.0 #20- sets setpoint in middle of dxf file for robot y axis #middle of dxf y axis is 0, so no centering needed here x_val = (20 - dxf_points[i][0]) * 0.0254 y_val = dxf_points[i][1] * 0.0254 pose_goal.position.x = 0.8 + y_val pose_goal.position.y = x_val pose_goal.position.z = 0.3 print(pose_goal) waypoints.append(pose_goal) """ euclidean_distances=[] for i in range(1,len(waypoints)): distance=pow(pow(waypoints[i].position.x-waypoints[i-1].position.x,2)+pow(waypoints[i].position.y-waypoints[i-1].position.y,2)+pow(waypoints[i].position.z-waypoints[i-1].position.z,2),0.5) print(distance) euclidean_distances.append(distance) error_code=None """ (plan, fraction) = self.move_group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print(type(plan)) #self.scene.motion_plan_request replan_value = 0 while (replan_value < 3 and fraction < 1.0): print(fraction) (plan, fraction) = self.move_group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold replan_value += 1 print("WARNING Portion of plan failed, replanning") if (replan_value > 2): return 0, 0, 0 #print(len(euclidean_distances)) #print(len(plan.joint_trajectory.points)) #print(waypoints[0]) #print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions)) #print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(3)) #print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(7)) #print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(11)) #print(plan.joint_trajectory.points) total_distance = 0 distances = [] for i in range(1, len(plan.joint_trajectory.points)): old_cartesian = self.kdl_kin.forward( plan.joint_trajectory.points[i - 1].positions) new_cartesian = self.kdl_kin.forward( plan.joint_trajectory.points[i].positions) distance = pow( pow(new_cartesian.item(3) - old_cartesian.item(3), 2) + pow(new_cartesian.item(7) - old_cartesian.item(7), 2) + pow(new_cartesian.item(11) - old_cartesian.item(11), 2), 0.5) distances.append(distance) total_distance += distance #new_time=plan.joint_trajectory.points[i].time_from_start.to_sec()+(distance/self.speed) #old_time=plan.joint_trajectory.points[i].time_from_start.to_sec() #print("new time") #print(new_time) #print(distance/self.speed) #print(old_time) #if(new_time > old_time): # plan.joint_trajectory.points[i].time_from_start.from_sec(new_time) #else: # print("Error, speed faster than possible execution time") print(total_distance) print(distances) total_time = plan.joint_trajectory.points[-1].time_from_start.to_sec() print(total_time) times = [] for i in distances: new_time = (i / total_distance) * total_time times.append(new_time) """ if(new_timestamp > old_timestamp) next_waypoint->time_from_start.fromSec(new_timestamp); else { //ROS_WARN_NAMED("setAvgCartesianSpeed", "Average speed is too fast. Moving as fast as joint constraints allow."); }""" #print point.time_from_start.secs #for i in range(len(plan.joint_trajectory.points)-1): # print("time between points:\n") # print(plan.joint_trajectory.points[i+1].time_from_start.nsecs-plan.joint_trajectory.points[i].time_from_start.nsecs) #for i in range(1,len(plan.joint_trajectory.points)-1): # plan.joint_trajectory.points[i].velocities=[-0.1,0.05,0.11,0.00000001,-0.05,0.2] #plan=self.move_group.retime_trajectory(moveit_robot_state,plan,velocity_scaling_factor=1.0, algorithm="iterative_spline_parameterization") if (self.display): self.display_robot_trajectory(plan) #self.actiongoal= control_msgs.msg.FollowJointTrajectoryActionGoal() self.goal = control_msgs.msg.FollowJointTrajectoryGoal() #self.actiongoal.header=std_msgs.msg.Header() self.goal.trajectory.joint_names = joint_state.name #self.goal.trajectory.points.resize(len(plan.joint_trajectory.points)) traj = Trajectory() time = 0.01 traj.add_point(plan.joint_trajectory.points[0].positions, time) #time=0.0 for i in range(1, len(plan.joint_trajectory.points)): time += times[i - 1] traj.add_point(plan.joint_trajectory.points[i].positions, time) #traj.start() #traj.wait(15.0) """ trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint() trajectory_point.positions=plan.joint_trajectory.points[i].positions trajectory_point.velocities=plan.joint_trajectory.points[i].velocities #trajectory_point.accelerations=plan.joint_trajectory.points[i].accelerations trajectory_point.time_from_start=plan.joint_trajectory.points[i].time_from_start self.goal.trajectory.points.append(trajectory_point) print(len(plan.joint_trajectory.points)) self.actiongoal.goal=self.goal self.followjointaction.publish(self.actiongoal) print("published goal") """ return plan, fraction, waypoints
def rpy_to_quaternion(self, rpy, dtype=np.float64): return self.q_to_quaternion(rox.R2q(rox.rpy2R(rpy)), dtype)
d.refresh_devices(1) d.connect_device("robotics_motion") d.connect_device("robot") c = d.get_device_client("robotics_motion", 1) #p_target = np.array([-np.random.uniform(0.4,0.8),np.random.uniform(-0.1,0.1),np.random.uniform(0.0,0.4)]) p_target = np.array([ -np.random.uniform(-0.1, 0.1), np.random.uniform(-0.1, 0.1), np.random.uniform(0.0, 0.4) ]) rpy_target = np.random.randn(3) * 0.5 rpy_target[0] += np.pi R_target = rox.rpy2R(rpy_target) # p_target = np.array([-0.6, 0.0, 0.1]) # R_target = np.array([[0,1,0],[1,0,0],[0,0,-1]]) T_target = rox.Transform(R_target, p_target) r = d.get_device_client("robot", 1) geom_util = GeometryUtil(client_obj=r) p_target = geom_util.rox_transform_to_pose(T_target) print("Begin movel") gen = c.movel("robot", p_target, "world", "robot_origin_calibration", 50) while True: try: gen.Next()