def get_des_twist(self, dT): """ Get desired twist @ time t, by computing differential of traj_frame @ t=T-1 and t=T """ if self.cur_wp_idx > 1: return PyKDL.diff(pm.fromMatrix(self.traj_list[self.cur_wp_idx - 1]), pm.fromMatrix(self.traj_list[self.cur_wp_idx]), dT) else: # cur_wp_idx == num_wp return PyKDL.diff(pm.fromMatrix(self.traj_list[self.cur_wp_idx]), pm.fromMatrix(self.traj_list[self.cur_wp_idx + 1]), dT)
def check_if_new_goal(self, frame): dist_p = kdl.diff(self.dst_pose.p, frame.p) dist_M = kdl.diff(self.dst_pose.M, frame.M) if dist_M[0] != dist_M[0]: #got a nan rospy.logwarn("Got a NAN in frame.M distance") return True dist_p_scalar = kdl.dot(dist_p, dist_p) dist_M_scalar = kdl.dot(dist_M, dist_M) if (dist_p_scalar > 0.00001) or (dist_M_scalar > 0.00001): return True else: return False
def compute_num_jac(self, func, ik_config, mat_world_to_obj): eps = 10**-6 jac = np.zeros((self.palm_dof_dim, self.arm_joints_dof)) for i in xrange(self.arm_joints_dof): ik_config_plus = np.copy(ik_config) ik_config_plus[i] += eps config_plus = func(ik_config_plus, mat_world_to_obj) ik_config_minus = np.copy(ik_config) ik_config_minus[i] -= eps config_minus = func(ik_config_minus, mat_world_to_obj) # ith_grad = (config_plus[:6] - config_minus[:6]) / (2. * eps) trans_plus = PyKDL.Vector(config_plus[0], config_plus[1], config_plus[2]) rot_plus = PyKDL.Rotation.RPY(config_plus[3], config_plus[4], config_plus[5]) frame_plus = PyKDL.Frame(rot_plus, trans_plus) trans_minus = PyKDL.Vector(config_minus[0], config_minus[1], config_minus[2]) rot_minus = PyKDL.Rotation.RPY(config_minus[3], config_minus[4], config_minus[5]) frame_minus = PyKDL.Frame(rot_minus, trans_minus) twist = PyKDL.diff(frame_minus, frame_plus, 1.0) ith_grad = np.array([ twist.vel[0], twist.vel[1], twist.vel[2], twist.rot[0], twist.rot[1], twist.rot[2] ]) / (2. * eps) jac[:, i] = ith_grad return jac
def _standing_still_for_x_seconds(self, timeout): """Check whether the robot is standing still for X seconds :param timeout how many seconds must the robot be standing still before returning True :type timeout float :returns bool indicating whether the robot has been standing still for longer than timeout seconds""" current_frame = self._robot.base.get_location().frame now = rospy.Time.now() if not self._last_pose_stamped: self._last_pose_stamped = current_frame self._last_pose_stamped_time = now else: current_yaw = current_frame.M.GetRPY()[2] # Get the Yaw last_yaw = self._last_pose_stamped.M.GetRPY()[2] # Get the Yaw # Compare the pose with the last pose and update if difference is larger than x if kdl.diff(current_frame.p, self._last_pose_stamped.p).Norm( ) > 0.05 or abs(current_yaw - last_yaw) > 0.3: # Update the last pose self._last_pose_stamped = current_frame self._last_pose_stamped_time = rospy.Time.now() else: print "Robot dit not move for x seconds: %f" % ( now - self._last_pose_stamped_time).to_sec() # Check whether we passed the timeout if (now - self._last_pose_stamped_time).to_sec() > timeout: return True return False
def calc_R(rx, ry, rz): R_7_M = PyKDL.Frame(PyKDL.Rotation.EulerZYX(rx, ry, rz)) ret = [] for idx in range(0,len(T_7bo_7i)): diff = PyKDL.diff( T_7bo_7i[idx] * R_7_M, R_7_M * T_Mbo_Mi[idx] ) ret.append( diff.rot.Norm() * weights_ori[idx] ) return ret
def _update(self, event): # Leave if ROS is not running or command is not valid if rospy.is_shutdown() or self._last_goal is None: return # Calculate the goal pose goal = self._get_goal() # TODO(mam0box) Test for singularities # End-effector's pose ee_pose = self._arm_interface.get_ee_pose_as_frame() # End-effector's velocity ee_vel = self._arm_interface.get_ee_vel_as_kdl_twist() # Calculate pose error error = PyKDL.diff(goal, ee_pose) # End-effector wrench to achieve target wrench = np.matrix(np.zeros(6)).T for i in range(len(wrench)): wrench[i] = -(self._Kp[i] * error[i] + self._Kd[i] * ee_vel[i]) # Compute jacobian transpose JT = self._arm_interface.jacobian_transpose() # Calculate the torques for the joints tau = JT * wrench # Store current pose target self._last_goal = goal self.publish_joint_efforts(tau)
def odomCallback(self, odom_msg): current_position = point_msg_to_kdl_vector(odom_msg.pose.pose.position) if self.active: self.distance_traveled += kdl.diff(current_position, self.previous_position).Norm() self.previous_position = current_position
def _standing_still_for_x_seconds(self, timeout): """Check whether the robot is standing still for X seconds :param timeout how many seconds must the robot be standing still before returning True :type timeout float :returns bool indicating whether the robot has been standing still for longer than timeout seconds""" current_frame = self._robot.base.get_location().frame now = rospy.Time.now() if not self._last_pose_stamped: self._last_pose_stamped = current_frame self._last_pose_stamped_time = now else: current_yaw = current_frame.M.GetRPY()[2] # Get the Yaw last_yaw = self._last_pose_stamped.M.GetRPY()[2] # Get the Yaw # Compare the pose with the last pose and update if difference is larger than x if kdl.diff(current_frame.p, self._last_pose_stamped.p).Norm() > 0.05 or abs(current_yaw - last_yaw) > 0.3: # Update the last pose self._last_pose_stamped = current_frame self._last_pose_stamped_time = rospy.Time.now() else: print "Robot dit not move for x seconds: %f"%(now - self._last_pose_stamped_time).to_sec() # Check whether we passed the timeout if (now - self._last_pose_stamped_time).to_sec() > timeout: return True return False
def getCartImpWristTraj(self, js, goal_T_B_W): init_js = copy.deepcopy(js) init_T_B_W = self.fk_solver.calculateFk("right_arm_7_link", init_js) T_B_Wd = goal_T_B_W T_B_W_diff = PyKDL.diff(init_T_B_W, T_B_Wd, 1.0) steps = int(max(T_B_W_diff.vel.Norm() / 0.05, T_B_W_diff.rot.Norm() / (20.0 / 180.0 * math.pi))) if steps < 2: steps = 2 self.updateJointLimits(init_js) self.updateMimicJoints(init_js) q_list = [] for f in np.linspace(0.0, 1.0, steps): T_B_Wi = PyKDL.addDelta(init_T_B_W, T_B_W_diff, f) q_out = self.fk_solver.simulateTrajectory("right_arm_7_link", init_js, T_B_Wi) if q_out == None: # print "error: getCartImpWristTraj: ", str(f) return None q_list.append(q_out) for i in range(7): joint_name = self.fk_solver.ik_joint_state_name["right_arm_7_link"][i] init_js[joint_name] = q_out[i] return q_list
def updateCom(T_B_O, T_B_O_2, contacts_O, com_pt, com_weights, m_id=0, pub_marker=None): diff_B = PyKDL.diff(T_B_O, T_B_O_2) up_v_B = PyKDL.Vector(0, 0, 1) n_v_B = diff_B.rot * up_v_B if pub_marker != None: m_id = pub_marker.publishVectorMarker( PyKDL.Vector(), diff_B.rot, m_id, r=0, g=1, b=0, frame="world", namespace="default", scale=0.01 ) m_id = pub_marker.publishVectorMarker( PyKDL.Vector(), n_v_B, m_id, r=1, g=1, b=1, frame="world", namespace="default", scale=0.01 ) n_O = PyKDL.Frame(T_B_O.Inverse().M) * n_v_B n_O_2 = PyKDL.Frame(T_B_O_2.Inverse().M) * n_v_B # get all com points that lies in the positive direction from the most negative contact point with respect to n_v in T_B_O and T_B_O_2 c_dot_list = [] c_dot_list_2 = [] for c in contacts_O: dot = PyKDL.dot(c, n_O) c_dot_list.append(dot) dot_2 = PyKDL.dot(c, n_O_2) c_dot_list_2.append(dot_2) # get all com points that lies in the positive direction from given contact for contact_idx in range(0, len(c_dot_list)): for com_idx in range(0, len(com_pt)): c = com_pt[com_idx] dot = PyKDL.dot(c, n_O) dot_2 = PyKDL.dot(c, n_O_2) if dot > c_dot_list[contact_idx] and dot_2 > c_dot_list_2[contact_idx]: com_weights[com_idx] += 1 return m_id
def handle_cart_cmd(self, msg): side = self.side try: # Get the current position of the hydra hydra_frame = fromTf(self.listener.lookupTransform( '/hydra_base', '/hydra_'+self.SIDE_STR[side]+'_grab', rospy.Time(0))) # Get the current position of the end-effector tip_frame = fromTf(self.listener.lookupTransform( '/world', self.tip_link, rospy.Time(0))) # Capture the current position if we're starting to move if not self.deadman_engaged: self.deadman_engaged = True self.cmd_origin = hydra_frame self.tip_origin = tip_frame else: self.deadman_max = max(self.deadman_max, msg.axes[self.DEADMAN[side]]) # Update commanded TF frame cmd_twist = kdl.diff(self.cmd_origin, hydra_frame) cmd_twist.vel = self.scale*self.deadman_max*cmd_twist.vel self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex: rospy.logwarn(str(ex))
def _update(self): # Leave if ROS is not running or command is not valid if rospy.is_shutdown() or self._last_goal is None: return # Calculate the goal pose goal = self._get_goal() # End-effector's pose ee_pose = self._arm_interface.get_ee_pose_as_frame() # End-effector's velocity ee_vel = self._arm_interface.get_ee_vel_as_kdl_twist() # Calculate pose error error = PyKDL.diff(goal, ee_pose) # End-effector wrench to achieve target wrench = np.matrix(np.zeros(6)).T for i in range(len(wrench)): wrench[i] = -(self._Kp[i] * error[i] + self._Kd[i] * ee_vel[i]) # Compute jacobian transpose JT = self._arm_interface.jacobian_transpose() # Calculate the torques for the joints tau = JT * wrench # Store current pose target self._last_goal = goal self.publish_goal() self.publish_joint_efforts(tau)
def handle_cart_cmd(self, scaling): """""" try: # Get the current position of the hydra input_frame = fromTf(self.listener.lookupTransform( self.input_ref_frame_id, self.input_frame_id, rospy.Time(0))) # Get the current position of the end-effector tip_frame = fromTf(self.listener.lookupTransform( self.input_ref_frame_id, self.tip_link, rospy.Time(0))) # Capture the current position if we're starting to move if not self.deadman_engaged: self.deadman_engaged = True self.cmd_origin = input_frame self.tip_origin = tip_frame else: self.cart_scaling = scaling # Update commanded TF frame cmd_twist = kdl.diff(self.cmd_origin, input_frame) cmd_twist.vel = self.scale*self.cart_scaling*cmd_twist.vel #rospy.logwarn(cmd_twist) self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex: rospy.logwarn(str(ex))
def moveRight(x, y, z, theta): modeCart() print "Moving right wrist to position:", x, y, z, "\n" T_B_Trd = PyKDL.Frame(PyKDL.Rotation.RPY(0.0, 0.0, theta), PyKDL.Vector(x, y, z)) if not velma.moveCartImpRight([T_B_Trd], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5) print "calculating difference between desiread and reached pose..." T_B_T_diff = PyKDL.diff(T_B_Trd, velma.getTf("B", "Tr"), 1.0) print T_B_T_diff if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05: releaseRight() (beer_frame, beer_angle) = locateObject("beer") moveRight(beer_frame.p[0] - 0.5 * math.cos(theta), beer_frame.p[1] - 0.5 * math.sin(theta), 0.5 * h_puszki + beer_frame.p[2], beer_angle) #podnosimy reke zeby nie potracic puszki grabRight(0) planAndExecute(q_default_position) exitError(10)
def torque_controller_cb(self, event): if rospy.is_shutdown() or self.cart_command == None: return elapsed_time = rospy.Time.now() - self.cart_command.header.stamp if elapsed_time.to_sec() > self.timeout: return # TODO: Validate msg.header.frame_id ## Cartesian error to zero using a Jacobian transpose controller x_target = posemath.fromMsg(self.cart_command.pose) q = self.get_joint_angles_array() x = baxter_to_kdl_frame(self.arm_interface.endpoint_pose()) xdot = baxter_to_kdl_twist(self.arm_interface.endpoint_velocity()) # Calculate a Cartesian restoring wrench x_error = PyKDL.diff(x_target, x) wrench = np.matrix(np.zeros(6)).T for i in range(len(wrench)): wrench[i] = -(self.kp[i] * x_error[i] + self.kd[i] * xdot[i]) # Calculate the jacobian J = self.kinematics.jacobian(q) # Convert the force into a set of joint torques. tau = J^T * wrench tau = J.T * wrench # Populate the joint_torques in baxter API format (dictionary) joint_torques = dict() for i, name in enumerate(self.joint_names): joint_torques[name] = tau[i] self.arm_interface.set_joint_torques(joint_torques)
def calc_R(px, py, pz): R_7_M = PyKDL.Frame(rot_mx, PyKDL.Vector(px, py, pz)) ret = [] for idx in range(0,len(T_7bp_7i)): diff = PyKDL.diff( T_7bp_7i[idx] * R_7_M, R_7_M * T_Mbp_Mi[idx] ) ret.append( diff.vel.Norm() * weights_pos[idx] ) return ret
def replan_movement(self, goal_kdl_frame, current_kdl_frame, interp_time=2): #Check if really new goal: if not self.check_if_new_goal(goal_kdl_frame): rospy.logwarn("Not a new goal") return #If we got here, the message is really different self.new_goal(goal_kdl_frame) self.set_current_pose(current_kdl_frame) #find distance rot_dist = kdl.diff(self.current_pose.M, self.dst_pose.M) if rot_dist[0] != rot_dist[0]: #getting nans here, work around bug by rotating current pose a little self.current_pose.M.DoRotZ(2 * 3.141509 / 180.0) dist = kdl.diff(kdl.Frame(self.current_pose), kdl.Frame(self.dst_pose)) #print("current pose:") #print self.current_pose.M.GetRPY() dt = 0.05 #looks smoother at 20Hz total_time = interp_time #sec times = int(total_time / dt) dx = 1.0 / times self.start_time = rospy.Time.now() self.pose_vector = [] for i in range(times + 1): if i == 0: inter_pose = self.current_pose else: inter_pose = kdl.addDelta(kdl.Frame(self.current_pose), dist, dx * i) inter_time = self.start_time + rospy.Duration(dt) * i #rospy.logwarn("Generated pose:") #print inter_pose #print inter_pose.p #print inter_pose.M.GetRPY() #rospy.logwarn("Original pose:") #print self.current_pose self.pose_vector.append([inter_time, kdl.Frame(inter_pose)])
def determine_points_of_interest(self, center_frame, z_max, convex_hull): """ Determine candidates for place poses :param center_frame: kdl.Frame, center of the Entity to place on top of :param z_max: float, height of the entity to place on, w.r.t. the entity :param convex_hull: [kdl.Vector], convex hull of the entity :return: [FrameStamped] of candidates for placing """ points = [] if len(convex_hull) == 0: rospy.logerr('determine_points_of_interest: Empty convex hull') return [] # Convert convex hull to map frame ch = offsetConvexHull(convex_hull, center_frame) # Loop over hulls self.marker_array.markers = [] for i in xrange(len(ch)): j = (i + 1) % len(ch) dx = ch[j].x() - ch[i].x() dy = ch[j].y() - ch[i].y() length = kdl.diff(ch[j], ch[i]).Norm() d = self._edge_distance while d < (length - self._edge_distance): # Point on edge xs = ch[i].x() + d / length * dx ys = ch[i].y() + d / length * dy # Shift point inwards and fill message fs = kdl_frame_stamped_from_XYZRPY( x=xs - dy / length * self._edge_distance, y=ys + dx / length * self._edge_distance, z=center_frame.p.z() + z_max, frame_id="/map") # It's nice to put an object on the middle of a long edge. In case of a cabinet, e.g., this might # prevent the robot from hitting the cabinet edges # print "Length: {}, edge score: {}".format(length, min(d, length-d)) setattr(fs, 'edge_score', min(d, length - d)) points += [fs] self.marker_array.markers.append( self.create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z())) # ToDo: check if still within hull??? d += self._spacing self.marker_pub.publish(self.marker_array) return points
def calc_rel_pose(source_pose_msg, target_pose_msg): (source_frame_id, source_frame) = poseStamped_to_kdlFrame(source_pose_msg) (target_frame_id, target_frame) = poseStamped_to_kdlFrame(target_pose_msg) if target_frame_id != source_frame_id: return None t = kdl.diff(target_frame, source_frame) return (t[0], t[1], t[2])
def determine_points_of_interest(self, center_frame, z_max, convex_hull): """ Determine candidates for place poses :param center_frame: kdl.Frame, center of the Entity to place on top of :param z_max: float, height of the entity to place on, w.r.t. the entity :param convex_hull: [kdl.Vector], convex hull of the entity :return: [FrameStamped] of candidates for placing """ points = [] if len(convex_hull) == 0: rospy.logerr('determine_points_of_interest: Empty convex hull') return [] # Convert convex hull to map frame ch = offsetConvexHull(convex_hull, center_frame) # Loop over hulls self.marker_array.markers = [] for i in xrange(len(ch)): j = (i + 1) % len(ch) dx = ch[j].x() - ch[i].x() dy = ch[j].y() - ch[i].y() length = kdl.diff(ch[j], ch[i]).Norm() d = self._edge_distance while d < (length - self._edge_distance): # Point on edge xs = ch[i].x() + d / length * dx ys = ch[i].y() + d / length * dy # Shift point inwards and fill message fs = kdl_frame_stamped_from_XYZRPY(x=xs - dy / length * self._edge_distance, y=ys + dx / length * self._edge_distance, z=center_frame.p.z() + z_max, frame_id="/map") # It's nice to put an object on the middle of a long edge. In case of a cabinet, e.g., this might # prevent the robot from hitting the cabinet edges # print "Length: {}, edge score: {}".format(length, min(d, length-d)) setattr(fs, 'edge_score', min(d, length-d)) points += [fs] self.marker_array.markers.append(self.create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z())) # ToDo: check if still within hull??? d += self._spacing self.marker_pub.publish(self.marker_array) return points
def integ_error(self, twist_err, dT=0.01): """Apply timestep-wise error integration. """ T = PyKDL.addDelta(self.prev_frame, twist_err, dT) # --> calculate the Integral #T = PyKDL.addDelta(self.init_frame, twist_err, self.traj_elapse) #return PyKDL.diff(self.init_frame, self.cur_frame, self.traj_elapse) return PyKDL.diff(self.init_frame, T, self.traj_elapse)
def calc_R(rx, ry, rz): R_mean = PyKDL.Frame(PyKDL.Rotation.EulerZYX(rx, ry, rz)) diff = [] for r in R: diff.append(PyKDL.diff( R_mean, r )) ret = [] for idx in range(0, len(diff)): rot_err = diff[idx].rot.Norm() ret.append(rot_err * wg[idx] / wg_sum) return ret
def calculateWrenchesForTransportTask(self, ext_wrenches_W, transport_T_B_O): ext_wrenches_O = [] for i in range(len(transport_T_B_O)-1): diff_B_O = PyKDL.diff(transport_T_B_O[i], transport_T_B_O[i+1]) # simulate object motion and calculate expected wrenches for t in np.linspace(0.0, 1.0, 5): T_B_Osim = PyKDL.addDelta(transport_T_B_O[i], diff_B_O, t) T_Osim_B = T_B_Osim.Inverse() for ewr in ext_wrenches_W: ext_wrenches_O.append(PyKDL.Frame(T_Osim_B.M) * ewr) return ext_wrenches_O
def moveCart(B_T): print "Zaczynam ruch nadgarstka" if not velma.moveCartImpRight([B_T], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5): exitError(16) if velma.waitForEffectorRight() != 0: exitError(17) rospy.sleep(0.5) print "calculating difference between desiread and reached pose..." T_B_T_diff = PyKDL.diff(B_T, velma.getTf("B", "Tr"), 1.0) print T_B_T_diff if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05: exitError(10)
def main(): print "\n\n==== To and from tf" tf = ((1,2,3), (1,0,0,0)) print tf print toTf(fromTf(tf)) print "\n\n==== To and from msg" p = Pose() p.orientation.x = 1 p.position.x = 4 print p print toMsg(fromMsg(p)) print "\n\n==== To and from matrix" m = numpy.array([[0,1,0,2], [0,0,1,3],[1,0,0,4],[0,0,0,1]]) print m print toMatrix(fromMatrix(m)) print "\n\n==== Math" print toTf( fromMsg(p) * fromMatrix(m)) print PyKDL.diff(fromMsg(p), fromMatrix(m)) print PyKDL.addDelta(fromMsg(p), PyKDL.diff(fromMatrix(m), fromMsg(p)), 2.0)
def within_tolerance(self, tip_pose, target_pose, scale=1.0): # compute cartesian command error tip_frame = fromTf(tip_pose) target_frame = fromTf(target_pose) twist_err = kdl.diff(tip_frame, target_frame) linear_err = twist_err.vel.Norm() angular_err = twist_err.rot.Norm() #print("linear: %g, angular %g" % (linear_err, angular_err)) # decide if planning is needed return linear_err < scale*self.linear_err_threshold and angular_err < scale*self.angular_err_threshold
def within_tolerance(self, tip_pose, target_pose, scale=1.0): # compute cartesian command error tip_frame = fromTf(tip_pose) target_frame = fromTf(target_pose) twist_err = kdl.diff(tip_frame, target_frame) linear_err = twist_err.vel.Norm() angular_err = twist_err.rot.Norm() #print("linear: %g, angular %g" % (linear_err, angular_err)) # decide if planning is needed return linear_err < scale * self.linear_err_threshold and angular_err < scale * self.angular_err_threshold
def checkGoal(self, q): # interpolate trajectory (in the cartesian space) self.openrave.robot_rave.SetDOFValues(q, self.dof_indices) link_E = self.openrave.robot_rave.GetLink("right_HandPalmLink") T_World_E = conv.OpenraveToKDL(link_E.GetTransform()) T_B_E = self.openrave.T_World_Br.Inverse() * T_World_E for T_B_Eg in self.goals_T_B_E: diff = PyKDL.diff(T_B_Eg, T_B_E) if diff.vel.Norm() <= 0.02 and diff.rot.Norm() <= 10.0/180.0*math.pi: return True return False
def setParams(self, param): ''' 0-15: homogeneous goal matrix 16: slow down distance ''' gpos = matrix([param[0:4], param[4:8], param[8:12], param[12:16]]) self.gp = gpos[0:3, 3].T.A[0] self.dToAttractor = length(self.p - self.gp) self.slowDownDistance = param[16] for i in range(3): for j in range(3): self.gframe.M[i, j] = gpos[i, j] self.tw = kdl.diff(self.frame, self.gframe)
def pi_controller(self, world): # get current state x = world.get_object_pose(self._object_name) xdot = world.get_object_twist(self._object_name) # calculate error terms e = kdl.diff(x, self._goal_pose) edot = self._goal_twist - xdot # calculate control wrench and apply it control_law = self._p_gain * e + self._d_gain * edot # return kdl.Wrench(control_law.vel, control_law.rot) return kdl.Wrench(control_law.vel, kdl.Vector()) # TODO: fix rot control
def getMovementTime2(self, T_B_Wd1, T_B_Wd2, max_v_l = 0.1, max_v_r = 0.2): twist = PyKDL.diff(T_B_Wd1, T_B_Wd2, 1.0) v_l = twist.vel.Norm() v_r = twist.rot.Norm() print "v_l: %s v_r: %s"%(v_l, v_r) f_v_l = v_l/max_v_l f_v_r = v_r/max_v_r if f_v_l > f_v_r: duration = f_v_l else: duration = f_v_r if duration < 0.5: duration = 0.5 return duration
def resolvedRates(self, currentPose, desiredPose): # compute pose error (result in kdl.twist format) poseError = PyKDL.diff(currentPose, desiredPose) posErrNorm = poseError.vel.Norm() rotErrNorm = poseError.rot.Norm() # compute velocity magnitude based on position error norm if posErrNorm > self.resolvedRatesConfig['tolPos']: tolPosition = self.resolvedRatesConfig['tolPos'] lambdaVel = self.resolvedRatesConfig['velRatio'] if posErrNorm > (lambdaVel * tolPosition): velMag = self.resolvedRatesConfig['velMax'] else: velMax = self.resolvedRatesConfig['velMax'] velMin = self.resolvedRatesConfig['velMin'] velMag = (velMin \ + (posErrNorm - tolPosition) \ * (velMax-velMin) / (tolPosition * (lambdaVel-1))) else: velMag = 0.0 # compute angular velocity based on rotation error norm if rotErrNorm > self.resolvedRatesConfig['tolRot']: tolRotation = self.resolvedRatesConfig['tolRot'] lambdaRot = self.resolvedRatesConfig['rotRatio'] if rotErrNorm > (lambdaRot * tolRotation): angVelMag = self.resolvedRatesConfig['angVelMax'] else: angVelMax = self.resolvedRatesConfig['angVelMax'] angVelMin = self.resolvedRatesConfig['angVelMin'] angVelMag = (angVelMin \ + (rotErrNorm - tolRotation) \ * (angVelMax - angVelMin) \ / (tolRotation * (lambdaRot-1))) else: angVelMag = 0.0 # The resolved rates is implemented as Nabil Simaan's notes # apply both the velocity and angular velocity in the error pose direction desiredTwist = PyKDL.Twist() poseError.vel.Normalize() # normalize to have the velocity direction desiredTwist.vel = poseError.vel * velMag poseError.rot.Normalize() # normalize to have the ang vel direction desiredTwist.rot = poseError.rot * angVelMag # Check whether we have reached our goal # goalReached = desiredTwist.vel.Norm() <= self.resolvedRatesConfig['velMin'] \ # and desiredTwist.rot.Norm() <= self.resolvedRatesConfig['angVelMin'] # Ignore rotation for calculating goalReached! goalReached = desiredTwist.vel.Norm( ) <= self.resolvedRatesConfig['velMin'] return desiredTwist, goalReached
def check_ik_result_using_fk(fk_solver, result_joint_state_kdl, goal_frame_kdl): fk_frame_kdl = PyKDL.Frame() fk_solver.JntToCart(result_joint_state_kdl, fk_frame_kdl) distance_vec = PyKDL.diff(fk_frame_kdl.p, goal_frame_kdl.p) norm = math.sqrt(PyKDL.dot(distance_vec, distance_vec)) print "distance norm: " + str(norm) relative_rotation = fk_frame_kdl.M.Inverse() * goal_frame_kdl.M distance_angle, _ = relative_rotation.GetRotAngle() print "distance angle: " + str(distance_angle) goal_pose_reached = (norm < 0.005) and (abs(distance_angle) < 0.1) return goal_pose_reached
def moveCart(T_B_Trd): # cart global move_time t=countTime(T_B_Trd) print("Moving right wrist to pose defined in world frame...") if not velma.moveCartImpRight([T_B_Trd], [move_time*t], None, None, None, None, makeWrench([5,5,5], [5,5,5]), start_time=0.01): exitError(13) if velma.waitForEffectorRight() != 0: exitError(14) rospy.sleep(0.5) print("Calculating difference between desiread and reached pose...") T_B_T_diff = PyKDL.diff(T_B_Trd, velma.getTf("B", "Tr"), 1.0) if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05: exitError(15)
def resolvedRates(config, currentPose, desiredPose): # compute pose error (result in kdl.twist format) poseError = PyKDL.diff(currentPose, desiredPose) posErrNorm = poseError.vel.Norm() rotErrNorm = poseError.rot.Norm() angVelMag = config['angVelMax'] if rotErrNorm < config['tolRot']: angVelMag = 0.0 elif rotErrNorm < angVelMag: angVelMag = rotErrNorm # compute velocity magnitude based on position error norm if posErrNorm > config['tolPos']: tolPosition = config['tolPos'] lambdaVel = config['velRatio'] if posErrNorm > (lambdaVel * tolPosition): velMag = config['velMax'] else: velMax = config['velMax'] velMin = config['velMin'] velMag = velMin + (posErrNorm - tolPosition) * \ (velMax - velMin)/(tolPosition*(lambdaVel-1)) else: velMag = 0.0 # compute angular velocity based on rotation error norm if rotErrNorm > config['tolRot']: tolRotation = config['tolRot'] lambdaRot = config['rotRatio'] if rotErrNorm > (lambdaRot * tolRotation): angVelMag = config['angVelMax'] else: angVelMax = config['angVelMax'] angVelMin = config['angVelMin'] angVelMag = angVelMin + (rotErrNorm - tolRotation) * \ (angVelMax - angVelMin)/(tolRotation*(lambdaRot-1)) else: angVelMag = 0.0 # The resolved rates is implemented as Nabil Simaan's notes # apply both the velocity and angular velocity in the error pose direction desiredTwist = PyKDL.Twist() poseError.vel.Normalize() # normalize to have the velocity direction desiredTwist.vel = poseError.vel * velMag poseError.rot.Normalize() # normalize to have the ang vel direction desiredTwist.rot = poseError.rot * angVelMag return desiredTwist
def get_err_twist(self, goal_frame=None, dT=0.01): """ Get the desirable twist for given error. """ #_cur_frame = self.get_current_frame() if self.btn_state_2 == 1: b_T_saw0 = pm.toMatrix(self.cur_frame) b_T_vr0 = pm.toMatrix(self._frame2) self.vr0_T_saw0 = np.matmul(inv(b_T_vr0), b_T_saw0) # numpy 4x4 array # else: # self.vr0_T_saw0 = np.identity(4) self._frame2 = pm.fromMatrix( np.matmul(self.vr0_T_saw0, pm.toMatrix(self._frame2))) return PyKDL.diff(self.cur_frame, self._frame2, dT)
def __init__(self): self.frame=kdl.Frame() self.gframe=kdl.Frame() self.gp=array([0,0,0]) self.p=array([0,0,0]) self.dToAttractor=0 self.slowDownDistance=0.05 #normal value 0.03 self.tw=kdl.diff(self.frame,self.gframe) self.rot_factor=1.0 self.min_rot=0.09 self.stopDistance=0.005 self.r_stopDistance=0.03 self.slowDownTime=0.1 self.slowDowninitTime=time.time() self.time=time.time() self.filter=[1.0]*5 self.filterC=array([0.05,0.3,0.3,0.3,0.05])
def move_end_effector_to_cartesian_position_and_angle_and_wrench( which_hand, angleX, angleY, angleZ, x, y, z, imp_list, imp_times, max_wrench, path_tool_tolerance, time): global velma T_hand = PyKDL.Frame(PyKDL.Rotation.RPY(angleX, angleY, angleZ), PyKDL.Vector(x, y, z)) if which_hand == RIGHT_HAND: if not velma.moveCartImpRight([T_hand], time, None, None, imp_list, imp_times, max_wrench, start_time=0.5, path_tol=path_tool_tolerance): exitError(13) error = velma.waitForEffectorRight() if error != 0: if error == PATH_TOLERANCE_VIOLATED_ERROR: return PATH_TOLERANCE_VIOLATED_ERROR elif which_hand == LEFT_HAND: if not velma.moveCartImpLeft([T_hand], [3.0], None, None, imp_list, imp_times, max_wrench, start_time=0.5, path_tol=path_tool_tolerance): exitError(13) error = velma.waitForEffectorLeft() if error != 0: if error == PATH_TOLERANCE_VIOLATED_ERROR: return PATH_TOLERANCE_VIOLATED_ERROR else: print "Error - none hand has been specified!" rospy.sleep(0.2) # Check precision if which_hand == RIGHT_HAND: T_hand_diff = PyKDL.diff(T_hand, velma.getTf("B", "Tr"), 1.0) if T_hand_diff.vel.Norm() > 0.05 or T_hand_diff.rot.Norm() > 0.05: print "Precision for right hand is not perfect!" elif which_hand == LEFT_HAND: print "d 1.13 - check precision for the left hand - later"
def compute_twist(T0, T1): """ Compute the twist between two homogeneous transformations: `twist = T1 - T0` Parameters ---------- T0: array_like Initial homogeneous transformation T1: array_like Final homogeneous transformation """ F0 = posemath.fromMatrix(T0) F1 = posemath.fromMatrix(T1) kdl_twist = PyKDL.diff(F0, F1) twist = np.zeros(6) twist[:3] = [kdl_twist.vel.x(), kdl_twist.vel.y(), kdl_twist.vel.z()] twist[3:] = [kdl_twist.rot.x(), kdl_twist.rot.y(), kdl_twist.rot.z()] return twist
def resolvedRates(self): # get current and desired robot pose (desired is the top of queue) currentPose = self.robot.get_current_position() desiredPose = self.trajectory[0] # compute pose error (result in kdl.twist format) poseError = PyKDL.diff(currentPose, desiredPose) posErrNorm = poseError.vel.Norm() rotErrNorm = poseError.rot.Norm() # compute velocity magnitude based on position error norm if posErrNorm > self.resolvedRatesConfig['tolPos']: tolPosition = self.resolvedRatesConfig['tolPos'] lambdaVel = self.resolvedRatesConfig['velRatio'] if posErrNorm > (lambdaVel * tolPosition): velMag = self.resolvedRatesConfig['velMax'] else: velMax = self.resolvedRatesConfig['velMax'] velMin = self.resolvedRatesConfig['velMin'] velMag = velMin + \ (velMax - velMin)/(tolPosition*(lambdaVel-1)) else: velMag = 0.0 # compute angular velocity based on rotation error norm if rotErrNorm > self.resolvedRatesConfig['tolRot']: tolRotation = self.resolvedRatesConfig['tolRot'] lambdaRot = self.resolvedRatesConfig['rotRatio'] if posErrNorm > (lambdaRot * tolRotation): angVelMag = self.resolvedRatesConfig['angVelMax'] else: angVelMax = self.resolvedRatesConfig['angVelMax'] angVelMin = self.resolvedRatesConfig['angVelMin'] angVelMag = angVelMin + \ (angVelMax - angVelMin)/(tolRotation*(lambdaRot-1)) else: angVelMag = 0.0 # The resolved rates is implemented as Nabil Simaan's notes # apply both the velocity and angular velocity in the error pose direction sys_dt = self.resolvedRatesConfig['dt'] desiredTwist = PyKDL.Twist() poseError.vel.Normalize() # normalize to have the velocity direction desiredTwist.vel = poseError.vel * velMag * sys_dt poseError.rot.Normalize() # normalize to have the ang vel direction desiredTwist.rot = poseError.rot * angVelMag * sys_dt return desiredTwist
def updateCom2(T_B_O, T_B_O_2, contacts_O, com_pt, com_weights): diff = PyKDL.diff(T_B_O, T_B_O_2) up_v = PyKDL.Vector(0, 0, 1) n_v = diff.rot * up_v n_O = T_B_O.Inverse() * n_v n_O_2 = T_B_O_2.Inverse() * n_v # get all com points that lies in the positive direction from the most negative contact point with respect to n_v in T_B_O and T_B_O_2 # get the minimum contact point for n_O min_c_dot = None for c in contacts_O: dot = PyKDL.dot(c, n_O) if min_c_dot == None or dot < min_c_dot: min_c_dot = dot # get all com points that lies in the positive direction from min_c_dot com_2_idx = [] for c_idx in range(0, len(com_pt)): c = com_pt[c_idx] dot = PyKDL.dot(c, n_O) if dot > min_c_dot: com_2_idx.append(c_idx) # get the minimum contact point for n_O min_c_dot = None for c in contacts_O: dot = PyKDL.dot(c, n_O_2) if min_c_dot == None or dot < min_c_dot: min_c_dot = dot # get all com points that lies in the positive direction from min_c_dot com_3_idx = [] for c_idx in com_2_idx: c = com_pt[c_idx] dot = PyKDL.dot(c, n_O_2) if dot > min_c_dot: com_3_idx.append(c_idx) for c_idx in range(0, len(com_pt)): com_weights[c_idx] -= 1 for c_idx in com_3_idx: com_weights[c_idx] += 2
def getVector(self,pos): ''' 0-15: position''' pos=matrix([pos[0:4],pos[4:8],pos[8:12],pos[12:16]]) self.p=pos[0:3,3].T.A[0] r=pos[0:3,0:3] #Cartesian vector self.dToAttractor=length(self.p-self.gp) if self.dToAttractor==0: P=array([0,0,0]) else: P=-(self.p-self.gp)/self.dToAttractor #todo: to improve stability we could decay near the attraction point #rotational twist for i in range(3): for j in range(3): self.frame.M[i,j]=r[i,j] self.tw=kdl.diff(self.frame,self.gframe) return concatenate((P,array([self.tw.rot[0],self.tw.rot[1],self.tw.rot[2]])),0)
def try_move(self, frame): #initialize joint values for search to current angles q_out = kdl.JntArray(self.jnt_pos) delta_q = kdl.JntArray(self.jnt_pos.rows()) maxiter = 300 eps = 0.002 f = Frame() searching = True iter = 0 while (searching): iter += 1 #forward kin of the current joints self.fk_solver.JntToCart(q_out, f) #distance in 6DOF between goal and current frame delta_twist = kdl.diff(f, frame) #velocity inv kinematics self.ikv_solver.CartToJnt(q_out, delta_twist, delta_q) #Add delta angle Add(q_out, delta_q, q_out) #Algorithm has converged: print "Delta_twist:" print delta_twist if ((abs(delta_twist.vel[0]) < eps) and (abs(delta_twist.vel[1]) < eps) and (abs(delta_twist.vel[2]) < eps)): print("found a solution") searching = False self.jnt_pos = q_out return (True) if (iter > maxiter): print "IK: Did not converge" return False
def get_des_twist(self, dT=0.01): """ Get desired twist @ time t, by computing differential of traj_frame @ t=T-1 and t=T """ #_cur_frame = self.get_current_frame() #return PyKDL.diff(self.prev_frame, _cur_frame, dT) # # if self.btn_state_2 == 1: b_T_saw0 = pm.toMatrix(self.cur_frame) b_T_vr0 = pm.toMatrix(self._frame2) self.vr0_T_saw0 = np.matmul(inv(b_T_vr0), b_T_saw0) # numpy 4x4 array # else: # self.vr0_T_saw0 = np.identity(4) self._frame2 = pm.fromMatrix( np.matmul(self.vr0_T_saw0, pm.toMatrix(self._frame2))) self._frame1 = pm.fromMatrix( np.matmul(self.vr0_T_saw0, pm.toMatrix(self._frame1))) return PyKDL.diff(self._frame1, self._frame2, dT)
def checkGoal(self, q): # interpolate trajectory (in the cartesian space) self.openrave.robot_rave.SetDOFValues(q, self.dof_indices) link_E = self.openrave.robot_rave.GetLink("right_HandPalmLink") T_World_E = conv.OpenraveToKDL(link_E.GetTransform()) T_B_O = self.openrave.T_World_Br.Inverse() * T_World_E * self.T_E_O diff = PyKDL.diff(self.T_B_O_nearHole, T_B_O) if diff.vel.Norm() > 0.02 or diff.rot.Norm() > 10.0/180.0*math.pi: return False angle1 = 0.0 for T_B_W, angle in self.key_traj1_T_B_W: init_js = self.openrave.getRobotConfigurationRos() traj = self.velma_solvers.getCartImpWristTraj(init_js, T_B_W) if traj == None: break angle1 = angle qar = {} for qi in range(len(traj[-1])): qar["right_arm_"+str(qi)+"_joint"] = traj[-1][qi] self.openrave.updateRobotConfigurationRos(qar) self.openrave.robot_rave.SetDOFValues(q, self.dof_indices) angle2 = 0.0 for T_B_W, angle in self.key_traj2_T_B_W: init_js = self.openrave.getRobotConfigurationRos() traj = self.velma_solvers.getCartImpWristTraj(init_js, T_B_W) if traj == None: break angle2 = angle qar = {} for qi in range(len(traj[-1])): qar["right_arm_"+str(qi)+"_joint"] = traj[-1][qi] self.openrave.updateRobotConfigurationRos(qar) if abs(angle1-angle2) > 190.0/180.0*math.pi: return True else: return False
def moveToInteractiveCursor(velma): T_Wo_test = velma.getTf("Wo", "example_frame") if not velma.moveCartImpRightCurrentPos(start_time=0.2): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) if not velma.moveCartImpRight([T_Wo_test], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5) T_B_T_diff = PyKDL.diff(T_Wo_test, velma.getTf("B", "Tr"), 1.0) if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05: exitError(10)
def torque_controller_cb(self, event): if rospy.is_shutdown() or None in [ self.cart_command, self.endpoint_state, self.joint_states ]: return # TODO: Validate msg.header.frame_id ## Cartesian error to zero using a Jacobian transpose controller x_target = posemath.fromMsg(self.cart_command.pose) q, qd, eff = self.kinematics.extract_joint_state(self.joint_states) x = posemath.fromMsg(self.endpoint_state.pose) xdot = TwistMsgToKDL(self.endpoint_state.twist) # Calculate a Cartesian restoring wrench x_error = PyKDL.diff(x_target, x) wrench = np.matrix(np.zeros(6)).T for i in range(len(wrench)): wrench[i] = -(self.kp[i] * x_error[i] + self.kd[i] * xdot[i]) # Calculate the jacobian J = self.kinematics.jacobian(q) # Convert the force into a set of joint torques. tau = J^T * wrench tau = J.T * wrench # Publish the joint_torques for i, name in enumerate(self.joint_names): self.torque_pub[name].publish(tau[i])
def execute_cb(self, goal): # helper variables success = True if self.robot_state.joint_impedance_active: self._result.error_code = 0 self._as.set_succeeded(self._result) print "MoveCartesianTrajectory ", self._action_name, " points: ", len(goal.trajectory.points) init_T_B_W = self.robot_state.fk_solver.calculateFk(self.wrist_name, self.robot_state.getJsPos()) init_T_B_T = init_T_B_W * self.robot_state.T_W_T[self.wrist_name] current_dest_point_idx = 0 while True: if self._as.is_preempt_requested(): rospy.loginfo("%s: Preempted" % self._action_name) self._as.set_preempted() return time_now = rospy.Time.now() time_from_start = (time_now - goal.trajectory.header.stamp).to_sec() if time_from_start <= 0: rospy.sleep(0.01) continue while time_from_start > goal.trajectory.points[current_dest_point_idx].time_from_start.to_sec(): current_dest_point_idx += 1 if current_dest_point_idx >= len(goal.trajectory.points): break if current_dest_point_idx >= len(goal.trajectory.points): break dest_time = goal.trajectory.points[current_dest_point_idx].time_from_start.to_sec() dest_T_B_T = pm.fromMsg(goal.trajectory.points[current_dest_point_idx].pose) if current_dest_point_idx > 0: prev_time = goal.trajectory.points[current_dest_point_idx - 1].time_from_start.to_sec() prev_T_B_T = pm.fromMsg(goal.trajectory.points[current_dest_point_idx - 1].pose) else: prev_time = 0.0 prev_T_B_T = init_T_B_T f = (time_from_start - prev_time) / (dest_time - prev_time) if f < 0 or f > 1: print "error: MoveCartesianTrajectory f: ", str(f) diff_T_B_T = PyKDL.diff(prev_T_B_T, dest_T_B_T, 1.0) T_B_Ti = PyKDL.addDelta(prev_T_B_T, diff_T_B_T, f) T_B_Wi = T_B_Ti * self.robot_state.T_W_T[self.wrist_name].Inverse() q_out = self.robot_state.fk_solver.simulateTrajectory(self.wrist_name, self.robot_state.getJsPos(), T_B_Wi) if q_out == None: self._result.error_code = -3 # PATH_TOLERANCE_VIOLATED self._as.set_aborted(self._result) return for i in range(7): joint_name = self.robot_state.fk_solver.ik_joint_state_name[self.wrist_name][i] self.robot_state.js.position[self.robot_state.joint_name_idx_map[joint_name]] = q_out[i] self.robot_state.arm_cmd[self.wrist_name] = T_B_Ti # publish the feedback self._feedback.header.stamp = time_now self._feedback.desired = pm.toMsg(T_B_Ti) self._feedback.actual = pm.toMsg(T_B_Ti) self._feedback.error = pm.toMsg(PyKDL.Frame()) self._as.publish_feedback(self._feedback) rospy.sleep(0.01) self._result.error_code = 0 self._as.set_succeeded(self._result)
print "Switch to cart_imp mode (no trajectory)..." if not velma.moveCartImpRightCurrentPos(start_time=0.2): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5) print "Moving right wrist to pose defined in world frame..." T_B_Trd = PyKDL.Frame(PyKDL.Rotation.RPY(0, -0.1, -0.2), PyKDL.Vector( 0.25 , -0.25 , 1.3 )) if not velma.moveCartImpRight([T_B_Trd], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5) print "calculating difference between desiread and reached pose..." T_B_T_diff = PyKDL.diff(T_B_Trd, velma.getTf("B", "Tr"), 1.0) print T_B_T_diff if T_B_T_diff.vel.Norm() > 0.1 or T_B_T_diff.rot.Norm() > 0.1: exitError(10) js = velma.getLastJointState() print js exitError(0)
def spin(self): rospack = rospkg.RosPack() model = "5dof" # model = "6dof" # model = "two_arms" if model == "6dof": urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator_6dof.urdf' srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator_6dof.srdf' elif model == "5dof": urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.urdf' srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.srdf' elif model == "two_arms": urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_two_arms.urdf' srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_two_arms.srdf' # TEST: line - line distance if False: while not rospy.is_shutdown(): pt = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0) line = (PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0), PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0)) line2 = (PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0), PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0)) dist, p1, p2 = self.distanceLines(line, line2) m_id = 0 m_id = self.pub_marker.publishVectorMarker(line[0], line[1], m_id, 0, 1, 0, frame='world', namespace='default', scale=0.02) m_id = self.pub_marker.publishVectorMarker(line2[0], line2[1], m_id, 1, 0, 0, frame='world', namespace='default', scale=0.02) m_id = self.pub_marker.publishVectorMarker(p1, p2, m_id, 1, 1, 1, frame='world', namespace='default', scale=0.02) print line, line2, dist raw_input(".") exit(0) # TEST: point - point distance if False: while not rospy.is_shutdown(): pt1 = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0) pt2 = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0) dist, p1, p2 = self.distancePoints(pt1, pt2) m_id = 0 m_id = self.pub_marker.publishSinglePointMarker(p1, m_id, r=0, g=1, b=0, a=1, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.1, 0.1, 0.1), T=None) m_id = self.pub_marker.publishSinglePointMarker(p2, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.1, 0.1, 0.1), T=None) m_id = self.pub_marker.publishVectorMarker(p1, p2, m_id, 1, 0, 0, frame='world', namespace='default', scale=0.02) print pt1, pt2, dist raw_input(".") exit(0) col = collision_model.CollisionModel() col.readUrdfSrdf(urdf_path, srdf_path) if model == "6dof": self.joint_names = ["0_joint", "1_joint", "2_joint", "3_joint", "4_joint", "5_joint"] effector_name = 'effector' elif model == "5dof": self.joint_names = ["0_joint", "1_joint", "2_joint", "3_joint", "4_joint"] effector_name = 'effector' elif model == "two_arms": self.joint_names = ["torso_0_joint", "left_0_joint", "left_1_joint", "left_2_joint", "left_3_joint", "right_0_joint", "right_1_joint", "right_2_joint", "right_3_joint"] effector_name = 'left_effector' self.q = np.zeros( len(self.joint_names) ) test_cases = [ (1.1, -2.3, 1.5, 1.5, 1.5), (-1.1, 2.3, -1.5, -1.5, -1.5), (0.0, 0.0, 1.5, 1.5, 1.5), (0.0, 0.0, -1.5, -1.5, -1.5), (0.2, 0.4, 1.5, 1.5, 1.5), (-0.2, -0.4, -1.5, -1.5, -1.5), ] # self.q[0] = 1.1 # self.q[1] = -2.3 # self.q[2] = 1.5 # self.q[3] = 1.5 # self.q[4] = 1.5 # self.q = np.array(test_cases[4]) solver = fk_ik.FkIkSolver(self.joint_names, [], None) self.publishJointState() # rospy.sleep(1) # exit(0) obst = [] obj = collision_model.CollisionModel.Collision() obj.type = "capsule" obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.5, 0.7, 0)) obj.radius = 0.1 obj.length = 0.4 obst.append( obj ) obj = collision_model.CollisionModel.Collision() obj.type = "capsule" obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.2, -0.9, 0)) obj.radius = 0.1 obj.length = 0.4 obst.append( obj ) obj = collision_model.CollisionModel.Collision() obj.type = "sphere" obj.T_L_O = PyKDL.Frame(PyKDL.Vector(1, 0, 0)) obj.radius = 0.05 obst.append( obj ) # rospy.sleep(1) # T_B_E = solver.calculateFk("base", "effector", self.q) # print T_B_E r_HAND_targets = [ PyKDL.Frame(PyKDL.Vector(0.1,1.0,0.0)), PyKDL.Frame(PyKDL.Vector(0.1,1.7,0.0)), # PyKDL.Frame(PyKDL.Rotation.RotY(90.0/180.0*math.pi), PyKDL.Vector(0.2,-0.5,0.0)), ] target_idx = 0 r_HAND_target = r_HAND_targets[target_idx] target_idx += 1 r_HAND_target = PyKDL.Frame(PyKDL.Vector(0.5,0.5,0)) last_time = rospy.Time.now() counter = 10000 while not rospy.is_shutdown(): if counter > 800: if model == "two_arms": r_HAND_target = PyKDL.Frame(PyKDL.Rotation.RotZ(random.uniform(-math.pi, math.pi)), PyKDL.Vector(random.uniform(-1,0), random.uniform(0,1.8), 0)) else: r_HAND_target = PyKDL.Frame(PyKDL.Rotation.RotZ(random.uniform(-math.pi, math.pi)), PyKDL.Vector(random.uniform(0,2), random.uniform(-1,1), 0)) # r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,-0.98100002989,0.194007580664), PyKDL.Vector(-0.129108034334,0.518606130706,0.0)) # r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.470814280381,0.882232346601), PyKDL.Vector(0.676567476122,0.0206561816531,0.0)) #r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.50451570265,0.863402516663), PyKDL.Vector(0.252380653828,0.923309935287,0.0)) #self.q = np.array( [-0.29968745 0.66939973 -2.49850991 1.87533697 2.63546305] ) #r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.704294768084,0.70990765572), PyKDL.Vector(0.334245569765,1.82368612057,0.0)) #self.q = np.array( [ 0.33203731 0.071835 -2.46646112 1.14339024 1.97684146] ) # r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.924467039084,-0.381261975087), PyKDL.Vector(0.261697539135,0.97235224304,0.0)) #r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.894763681298,0.446539980999), PyKDL.Vector(0.354981453046,0.604598917063,0.0)) #self.q = np.array( [-0.89640518 0.44336642 1.96125279 -1.66533209 -2.19189403] ) qt = r_HAND_target.M.GetQuaternion() pt = r_HAND_target.p print "r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s), PyKDL.Vector(%s,%s,%s))"%(qt[0],qt[1],qt[2],qt[3],pt[0],pt[1],pt[2]) print "self.q = np.array(", self.q, ")" counter = 0 counter += 1 time_elapsed = rospy.Time.now() - last_time J_JLC = np.matrix(numpy.zeros( (len(self.q), len(self.q)) )) delta_V_JLC = np.empty(len(self.q)) for q_idx in range(len(self.q)): if self.q[q_idx] < solver.lim_lower_soft[q_idx]: delta_V_JLC[q_idx] = self.q[q_idx] - solver.lim_lower_soft[q_idx] J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_lower_soft[q_idx]) / abs(solver.lim_lower[q_idx] - solver.lim_lower_soft[q_idx])) elif self.q[q_idx] > solver.lim_upper_soft[q_idx]: delta_V_JLC[q_idx] = self.q[q_idx] - solver.lim_upper_soft[q_idx] J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_upper_soft[q_idx]) / abs(solver.lim_upper[q_idx] - solver.lim_upper_soft[q_idx])) else: delta_V_JLC[q_idx] = 0.0 J_JLC[q_idx,q_idx] = 0.0 J_JLC_inv = J_JLC.transpose()#np.linalg.pinv(J_JLC) N_JLC = np.matrix(np.identity(len(self.q))) - (J_JLC_inv * J_JLC) N_JLC_inv = np.linalg.pinv(N_JLC) v_max_JLC = 20.0/180.0*math.pi kp_JLC = 10.0 dx_JLC_des = kp_JLC * delta_V_JLC # min(1.0, v_max_JLC/np.linalg.norm(dx_JLC_des)) if v_max_JLC > np.linalg.norm(dx_JLC_des): vv_JLC = 1.0 else: vv_JLC = v_max_JLC/np.linalg.norm(dx_JLC_des) dx_JLC_ref = - vv_JLC * dx_JLC_des J_r_HAND = solver.getJacobian('base', effector_name, self.q, base_end=False) J_r_HAND_inv = np.linalg.pinv(J_r_HAND) T_B_E = solver.calculateFk('base', effector_name, self.q) r_HAND_current = T_B_E r_HAND_diff = PyKDL.diff(r_HAND_current, r_HAND_target) delta_V_HAND = np.empty(6) delta_V_HAND[0] = r_HAND_diff.vel[0] delta_V_HAND[1] = r_HAND_diff.vel[1] delta_V_HAND[2] = r_HAND_diff.vel[2] delta_V_HAND[3] = r_HAND_diff.rot[0] delta_V_HAND[4] = r_HAND_diff.rot[1] delta_V_HAND[5] = r_HAND_diff.rot[2] v_max_HAND = 4.0 kp_HAND = 10.0 dx_HAND_des = kp_HAND * delta_V_HAND if v_max_HAND > np.linalg.norm(dx_HAND_des): vv_HAND = 1.0 else: vv_HAND = v_max_HAND/np.linalg.norm(dx_HAND_des) dx_r_HAND_ref = vv_HAND * dx_HAND_des links_fk = {} for link in col.links: links_fk[link.name] = solver.calculateFk('base', link.name, self.q) activation_dist = 0.05 link_collision_map = {} if True: # self collision total_contacts = 0 for link1_name, link2_name in col.collision_pairs: link1 = col.link_map[link1_name] T_B_L1 = links_fk[link1_name] link2 = col.link_map[link2_name] T_B_L2 = links_fk[link2_name] for col1 in link1.col: for col2 in link2.col: T_B_C1 = T_B_L1 * col1.T_L_O T_B_C2 = T_B_L2 * col2.T_L_O c_dist = (T_B_C1 * PyKDL.Vector() - T_B_C2 * PyKDL.Vector()).Norm() if col1.type == "capsule": c_dist -= col1.radius + col1.length/2 elif col1.type == "sphere": c_dist -= col1.radius if col2.type == "capsule": c_dist -= col2.radius + col2.length/2 elif col2.type == "sphere": c_dist -= col2.radius if c_dist > activation_dist: continue dist = None if col1.type == "capsule" and col2.type == "capsule": line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distanceLines(line1, line2) elif col1.type == "capsule" and col2.type == "sphere": line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) pt = T_B_C2 * PyKDL.Vector() dist, p1_B, p2_B = self.distanceLinePoint(line, pt) elif col1.type == "sphere" and col2.type == "capsule": pt = T_B_C1 * PyKDL.Vector() line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distancePointLine(pt, line) elif col1.type == "sphere" and col2.type == "sphere": dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector()) else: print "ERROR: unknown collision type:", col1.type, col2.type exit(0) if dist != None: # print "dist", dist, link1_name, link2_name, col1.type, col2.type dist -= col1.radius + col2.radius v = p2_B - p1_B v.Normalize() n1_B = v n2_B = -v p1_B += n1_B * col1.radius p2_B += n2_B * col2.radius if dist < activation_dist: if not (link1_name, link2_name) in link_collision_map: link_collision_map[(link1_name, link2_name)] = [] link_collision_map[(link1_name, link2_name)].append( (p1_B, p2_B, dist, n1_B, n2_B) ) # environment collisions for link in col.links: if link.col == None: continue link1_name = link.name T_B_L1 = links_fk[link1_name] T_B_L2 = links_fk["base"] for col1 in link.col: for col2 in obst: T_B_C1 = T_B_L1 * col1.T_L_O T_B_C2 = T_B_L2 * col2.T_L_O dist = None if col1.type == "capsule" and col2.type == "capsule": line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distanceLines(line1, line2) elif col1.type == "capsule" and col2.type == "sphere": line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) pt = T_B_C2 * PyKDL.Vector() dist, p1_B, p2_B = self.distanceLinePoint(line, pt) elif col1.type == "sphere" and col2.type == "capsule": pt = T_B_C1 * PyKDL.Vector() line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distancePointLine(pt, line) elif col1.type == "sphere" and col2.type == "sphere": dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector()) # print "a:",dist, p1_B, p2_B else: print "ERROR: unknown collision type:", col1.type, col2.type exit(0) if dist != None: dist -= col1.radius + col2.radius v = p2_B - p1_B v.Normalize() n1_B = v n2_B = -v p1_B += n1_B * col1.radius p2_B += n2_B * col2.radius # if col1.type == "sphere" and col2.type == "sphere": # print "b:",dist, p1_B, p2_B if dist < activation_dist: if not (link1_name, "base") in link_collision_map: link_collision_map[(link1_name, "base")] = [] link_collision_map[(link1_name, "base")].append( (p1_B, p2_B, dist, n1_B, n2_B) ) omega_col = np.matrix(np.zeros( (len(self.q),1) )) Ncol = np.matrix(np.identity(len(self.q))) m_id = 0 for link1_name, link2_name in link_collision_map: for (p1_B, p2_B, dist, n1_B, n2_B) in link_collision_map[ (link1_name, link2_name) ]: if dist > 0.0: m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 1, 1, frame='base', namespace='default', scale=0.02) else: m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 0, 0, frame='base', namespace='default', scale=0.02) T_B_L1 = links_fk[link1_name] T_L1_B = T_B_L1.Inverse() T_B_L2 = links_fk[link2_name] T_L2_B = T_B_L2.Inverse() p1_L1 = T_L1_B * p1_B p2_L2 = T_L2_B * p2_B n1_L1 = PyKDL.Frame(T_L1_B.M) * n1_B n2_L2 = PyKDL.Frame(T_L2_B.M) * n2_B # print p1_L1, p1_L1+n1_L1*0.2 # print p2_L2, p2_L2+n2_L2*0.2 # m_id = self.pub_marker.publishVectorMarker(p1_L1, p1_L1+n1_L1*0.2, m_id, 0, 1, 0, frame=link1_name, namespace='default', scale=0.02) # m_id = self.pub_marker.publishVectorMarker(T_B_L2*p2_L2, T_B_L2*(p2_L2+n2_L2*0.2), m_id, 0, 0, 1, frame="base", namespace='default', scale=0.02) jac1 = PyKDL.Jacobian(len(self.q)) jac2 = PyKDL.Jacobian(len(self.q)) common_link_name = solver.getJacobiansForPairX(jac1, jac2, link1_name, p1_L1, link2_name, p2_L2, self.q, None) # T_B_Lc = links_fk[common_link_name] # jac1.changeBase(T_B_Lc.M) # jac2.changeBase(T_B_Lc.M) # T_Lc_L1 = T_B_Lc.Inverse() * T_B_L1 # T_Lc_L2 = T_B_Lc.Inverse() * T_B_L2 # n1_Lc = PyKDL.Frame(T_Lc_L1.M) * n1_L1 # n2_Lc = PyKDL.Frame(T_Lc_L2.M) * n2_L2 # print link1_name, link2_name # print "jac1" # print jac1 # print "jac2" # print jac2 # repulsive velocity V_max = 20.0 dist = max(dist, 0.0) depth = (activation_dist - dist) # Vrep = V_max * depth * depth / (activation_dist * activation_dist) Vrep = V_max * depth / activation_dist # Vrep = -max(Vrep, 0.01) Vrep = -Vrep # # the mapping between motions along contact normal and the Cartesian coordinates e1 = n1_L1 e2 = n2_L2 Jd1 = np.matrix([e1[0], e1[1], e1[2]]) Jd2 = np.matrix([e2[0], e2[1], e2[2]]) # rewrite the linear part of the jacobian jac1_mx = np.matrix(np.zeros( (3, len(self.q)) )) jac2_mx = np.matrix(np.zeros( (3, len(self.q)) )) for q_idx in range(len(self.q)): col1 = jac1.getColumn(q_idx) col2 = jac2.getColumn(q_idx) for row_idx in range(3): jac1_mx[row_idx, q_idx] = col1[row_idx] jac2_mx[row_idx, q_idx] = col2[row_idx] Jcol1 = Jd1 * jac1_mx Jcol2 = Jd2 * jac2_mx Jcol = np.matrix(np.zeros( (2, len(self.q)) )) for q_idx in range(len(self.q)): Jcol[0, q_idx] = Jcol1[0, q_idx] Jcol[1, q_idx] = Jcol2[0, q_idx] # Jcol = Jcol * (Ncol * N_JLC) # TODO: is the transposition ok? Jcol_pinv = np.linalg.pinv(Jcol) # Jcol_pinv = Jcol.transpose() activation = min(1.0, 2.0*depth/activation_dist) a_des = np.matrix(np.zeros( (len(self.q),len(self.q)) )) a_des[0,0] = a_des[1,1] = activation U, S, V = numpy.linalg.svd(Jcol, full_matrices=True, compute_uv=True) # print "activation", activation # print "Jcol" # print Jcol # raw_input(".") if rospy.is_shutdown(): exit(0) # print "V" # print V # print "S" # print S # Ncol12 = np.matrix(np.identity(len(self.q))) - Jcol.transpose() * (Jcol_pinv).transpose() Ncol12 = np.matrix(np.identity(len(self.q))) - (V * a_des * V.transpose()) Ncol = Ncol * Ncol12 # d_omega = Jcol_prec_inv * np.matrix([Vrep, Vrep]).transpose() d_omega = Jcol_pinv * np.matrix([Vrep, Vrep]).transpose() omega_col += d_omega # print "omega_col", omega_col # print dx_HAND_ref # omega_r_HAND = (J_r_HAND_inv * np.matrix(dx_r_HAND_ref).transpose()) self.pub_marker.eraseMarkers(m_id, 10, frame_id='base', namespace='default') Ncol_inv = np.linalg.pinv(Ncol) J_r_HAND_prec = J_r_HAND * (Ncol * N_JLC) J_r_HAND_prec_inv = np.linalg.pinv(J_r_HAND_prec) omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + N_JLC.transpose() * (omega_col + (Ncol.transpose() * J_r_HAND_inv) * np.matrix(dx_r_HAND_ref).transpose()) # omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + N_JLC_inv * (omega_col + (Ncol_inv * J_r_HAND_prec_inv) * np.matrix(dx_r_HAND_ref).transpose()) # omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + np.linalg.pinv(N_JLC) * omega_col # omega = omega_col omega_vector = np.empty(len(self.q)) for q_idx in range(len(self.q)): omega_vector[q_idx] = omega[q_idx][0] max_norm = 0.5 omega_norm = np.linalg.norm(omega_vector) if omega_norm > max_norm: omega_vector *= max_norm / omega_norm self.q += omega_vector * 0.02 if time_elapsed.to_sec() > 0.05: # print self.q m_id = 0 m_id = self.publishRobotModelVis(m_id, col, links_fk) m_id = self.publishObstaclesVis(m_id, obst) last_time = rospy.Time.now() self.publishJointState() self.publishTransform(r_HAND_target, "hand_dest")
def find_checkerboard_service(self, req): poses = [] min_x = self.ros_image.width min_y = self.ros_image.height max_x = 0 max_y = 0 for i in range(10): rospy.sleep(0.5) #copy the image over with self.mutex: image = self.ros_image result = self.find_checkerboard_pose(image, req.corners_x, req.corners_y, req.spacing_x, req.spacing_y, self.width_scaling, self.height_scaling) if result is None: rospy.logerr("Couldn't find a checkerboard") continue p, corners = result poses.append(p) for j in range(corners.cols): x = cv.GetReal2D(corners, 0, j) y = cv.GetReal2D(corners, 1, j) min_x = min(min_x, x) max_x = max(max_x, x) min_y = min(min_y, y) max_y = max(max_y, y) # convert all poses to twists twists = [] for p in poses: twists.append( PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose))) # get average twist twist_res = PyKDL.Twist() PyKDL.SetToZero(twist_res) for t in twists: for i in range(3): twist_res.vel[i] += t.vel[i] / len(poses) twist_res.rot[i] += t.rot[i] / len(poses) # get noise noise_rot = 0 noise_vel = 0 for t in twists: n_vel = (t.vel - twist_res.vel).Norm() n_rot = (t.rot - twist_res.rot).Norm() if n_vel > noise_vel: noise_vel = n_vel if n_rot > noise_rot: noise_rot = n_rot # set service resul pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0) pose_msg = PoseStamped() pose_msg.header = poses[0].header pose_msg.pose = posemath.toMsg(pose_res) return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y, max_y, noise_vel, noise_rot)
def spin(self): # locate all markers on the object if False: max_angle = 60.0/180.0*math.pi min_z = math.cos(max_angle) print "min_z: %s"%(min_z) # big_box # marker_list = [18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31] # small_box # marker_list = [32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45] # table # marker_list = [3,4,5,6] # c-beam marker_list = [46, 47, 48, 49, 50, 51, 52, 53, 54, 55] mtrans_matrix = [] mtrans_weights_ori = [] mtrans_weights_pos = [] for i in range(0, len(marker_list)): mtrans_matrix.append([]) mtrans_weights_ori.append([]) mtrans_weights_pos.append([]) for j in range(0, len(marker_list)): if i > j: mtrans_matrix[i].append([]) mtrans_weights_ori[i].append([]) mtrans_weights_pos[i].append([]) else: mtrans_matrix[i].append(None) mtrans_weights_ori[i].append(None) mtrans_weights_pos[i].append(None) marker_lock = Lock() self.visible_markers = [] def markerCallback(data): marker_lock.acquire() self.visible_markers = [] for m in data.markers: if m.id in marker_list: self.visible_markers.append( (m.id, pm.fromMsg(m.pose.pose)) ) marker_lock.release() rospy.Subscriber('/ar_pose_marker', AlvarMarkers, markerCallback) while not rospy.is_shutdown(): marker_lock.acquire() visible_markers_local = copy.copy(self.visible_markers) marker_lock.release() accepted_markers = [] accepted_markers_id = [] accepted_markers_weights_pos = [] accepted_markers_weights_ori = [] for m in visible_markers_local: n = PyKDL.Frame(m[1].M) * PyKDL.Vector(0,0,-1) if n.z() > min_z: accepted_markers.append( m ) accepted_markers_id.append( m[0] ) # n.z() is in range (min_z, 1.0), min_z is the best for orientation and 1.0 i best for position weight_pos = (n.z() - min_z)/(1.0-min_z) if weight_pos > 1.0 or weight_pos < 0.0: print "error: weight==%s"%(weight_pos) accepted_markers_weights_pos.append(weight_pos) accepted_markers_weights_ori.append(1.0-weight_pos) print "visible: %s accepted markers: %s"%(len(visible_markers_local), accepted_markers_id) for i in range(0, len(accepted_markers)): for j in range(i+1, len(accepted_markers)): if accepted_markers[i][0] > accepted_markers[j][0]: idx1 = i idx2 = j else: idx1 = j idx2 = i # print " %s with %s"%(accepted_markers[idx1][0], accepted_markers[idx2][0]) T_C_M1 = accepted_markers[idx1][1] T_C_M2 = accepted_markers[idx2][1] T_M1_M2 = T_C_M1.Inverse() * T_C_M2 marker_id1_index = marker_list.index(accepted_markers[idx1][0]) marker_id2_index = marker_list.index(accepted_markers[idx2][0]) mtrans_matrix[marker_id1_index][marker_id2_index].append(T_M1_M2) mtrans_weights_ori[marker_id1_index][marker_id2_index].append(accepted_markers_weights_ori[idx1] * accepted_markers_weights_ori[idx2]) mtrans_weights_pos[marker_id1_index][marker_id2_index].append(accepted_markers_weights_pos[idx1] * accepted_markers_weights_pos[idx2]) rospy.sleep(0.1) G = {} for i in range(0, len(marker_list)): for j in range(0, len(marker_list)): if mtrans_matrix[i][j] == None: pass # print "%s with %s: None"%(marker_list[i], marker_list[j]) elif len(mtrans_matrix[i][j]) < 10: print "%s with %s: %s (ignoring)"%(marker_list[i], marker_list[j], len(mtrans_matrix[i][j])) else: score, R_M1_M2 = velmautils.meanOrientation(mtrans_matrix[i][j], mtrans_weights_ori[i][j]) # ignore 20% the most distant measures from the mean distances = [] for measure_idx in range(0, len(mtrans_matrix[i][j])): diff = PyKDL.diff(R_M1_M2, mtrans_matrix[i][j][measure_idx]).rot.Norm() distances.append([diff, measure_idx]) distances_sorted = sorted(distances, key=operator.itemgetter(0)) mtrans = [] weights_ori = [] for measure_idx in range(0, int(0.8*len(mtrans_matrix[i][j]))): mtrans.append(mtrans_matrix[i][j][distances_sorted[measure_idx][1]]) weights_ori.append(mtrans_weights_ori[i][j][distances_sorted[measure_idx][1]]) # score, R_M1_M2 = velmautils.meanOrientation(mtrans, weights_ori) print "%s with %s: %s, score: %s"%(marker_list[i], marker_list[j], len(mtrans_matrix[i][j]), score) P_M1_M2 = velmautils.meanPosition(mtrans_matrix[i][j], mtrans_weights_pos[i][j]) print "P_M1_M2 before: %s"%(P_M1_M2) # ignore 20% the most distant measures from the mean distances = [] for measure_idx in range(0, len(mtrans_matrix[i][j])): diff = PyKDL.diff(R_M1_M2, mtrans_matrix[i][j][measure_idx]).vel.Norm() distances.append([diff, measure_idx]) distances_sorted = sorted(distances, key=operator.itemgetter(0)) mtrans = [] weights_pos = [] for measure_idx in range(0, int(0.8*len(mtrans_matrix[i][j]))): mtrans.append(mtrans_matrix[i][j][distances_sorted[measure_idx][1]]) weights_pos.append(mtrans_weights_ori[i][j][distances_sorted[measure_idx][1]]) # P_M1_M2 = velmautils.meanPosition(mtrans, weights_pos) print "P_M1_M2 after: %s"%(P_M1_M2) mtrans_matrix[i][j] = PyKDL.Frame(copy.deepcopy(R_M1_M2.M), P_M1_M2) neighbours = G.get(marker_list[i], {}) if score > 0.01: score = 1000000.0 else: score = 1.0 neighbours[marker_list[j]] = score G[marker_list[i]] = neighbours neighbours = G.get(marker_list[j], {}) neighbours[marker_list[i]] = score G[marker_list[j]] = neighbours frames = [] # get the shortest paths weighted by score from optimization for marker_id in marker_list: path = dijkstra.shortestPath(G,marker_id,marker_list[0]) print path T_Ml_Mf = PyKDL.Frame() for i in range(0, len(path)-1): if marker_list.index(path[i]) > marker_list.index(path[i+1]): T_Mp_Mn = mtrans_matrix[marker_list.index(path[i])][marker_list.index(path[i+1])] T_Ml_Mf = T_Ml_Mf * T_Mp_Mn else: T_Mn_Mp = mtrans_matrix[marker_list.index(path[i+1])][marker_list.index(path[i])] T_Ml_Mf = T_Ml_Mf * T_Mn_Mp.Inverse() frames.append(copy.deepcopy(T_Ml_Mf.Inverse())) pub_marker = velmautils.MarkerPublisher() rospy.sleep(1.0) m_id = 0 marker_idx = 0 for fr in frames: q = fr.M.GetQuaternion() p = fr.p print "[%s, PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s),PyKDL.Vector(%s,%s,%s))],"%(marker_list[marker_idx], q[0], q[1], q[2], q[3], p.x(), p.y(), p.z()) m_id = pub_marker.publishFrameMarker(fr, m_id, scale=0.1, frame='world', namespace='default') rospy.sleep(0.1) marker_idx += 1 rospy.sleep(2.0) exit(0) else: pub_marker = velmautils.MarkerPublisher() rospy.sleep(1.0) markers2 = [ [46, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.0,1.0),PyKDL.Vector(-0.0,-0.0,-0.0))], [47, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0161417497092,-9.92379339669e-05,-0.00603105214296,0.999851519216),PyKDL.Vector(-0.0987990318312,-0.000265582834399,0.000544628226204))], [48, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.706829255712,0.00114672638679,-0.707103949357,0.0198769487466),PyKDL.Vector(0.0427261427894,0.00245374838957,-0.116698579624))], [49, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.706230029879,-0.00660144281521,-0.70769079068,0.0192174565616),PyKDL.Vector(0.0410352237403,0.000595788239244,-0.0336956438972))], [50, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0155276433453,0.714580229904,0.00743395758828,0.699341635824),PyKDL.Vector(-0.131991504795,-0.00410930997885,-0.0916799439467))], [51, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.706160148032,0.0114839861434,-0.707838133957,0.0130820300375),PyKDL.Vector(-0.150701418215,-0.000688426198715,-0.035762545196))], [52, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.705850422242,0.00260066465892,-0.708005925475,0.022271673869),PyKDL.Vector(-0.150663515172,-0.00196494029406,-0.123356224597))], [53, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00630495805624,-0.999979097775,0.000308879730173,0.00139860956497),PyKDL.Vector(-0.0116053782242,0.000352840524022,-0.0267278839783))], [54, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00853722085061,-0.999853611966,0.00230495916657,0.0146477869582),PyKDL.Vector(-0.0949889134574,0.00131718330416,-0.0165548984986))], [55, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0208301706621,-0.711731154203,0.0052945617051,0.702123091589),PyKDL.Vector(0.0244779541548,0.000463479220587,-0.0804749548707))], ] markers = [ [46, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.0,1.0),PyKDL.Vector(-0.0,-0.0,-0.0))], [47, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00211977224419,0.00133104595822,-0.00433238039783,0.999987482603),PyKDL.Vector(-0.0995553679408,-0.000651966932258,0.000444468330964))], [48, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.705539374795,0.00129956423061,-0.708394191186,0.0197527628665),PyKDL.Vector(0.0433256677966,0.00212664651843,-0.116482343501))], [49, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.704707757517,-0.00628228374428,-0.709255128279,0.0174548680028),PyKDL.Vector(0.0412709849952,0.000494665961165,-0.0338667341872))], [50, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0117276773848,0.706312439798,0.00558379104701,0.707781053891),PyKDL.Vector(-0.131246837996,-0.00469319026484,-0.0943814089463))], [51, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.710112841604,0.00963407546503,-0.703895468304,0.013345653983),PyKDL.Vector(-0.149984963191,-0.00300459973807,-0.0370193446712))], [52, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.704691425649,0.00497982940017,-0.709159595229,0.0218601100464),PyKDL.Vector(-0.15277553848,-0.00431480401095,-0.120995842686))], [53, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00984066000086,-0.999945658681,0.00299307949816,0.00169781765667),PyKDL.Vector(-0.0132269965227,-0.00110379001368,-0.0274175040768))], [54, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00154140261629,0.999633307109,-0.00751679824708,0.0259686953912),PyKDL.Vector(-0.0974091549673,-0.000670004842722,-0.0319416169884))], [55, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0195903374145,-0.713404165076,0.00273342374532,0.700473585745),PyKDL.Vector(0.0250633176495,-0.00356911439713,-0.0811403242495))], ] m_id = 0 for m in markers: print m[0] print m[1] m_id = pub_marker.publishFrameMarker(m[1], m_id, scale=0.1, frame='world', namespace='default') rospy.sleep(0.1)
def spin(self): if True: # Create a world object world = ode.World() # world.setGravity( (0,0,-3.81) ) world.setGravity( (0,0,0) ) CFM = world.getCFM() ERP = world.getERP() print "CFM: %s ERP: %s"%(CFM, ERP) # world.setCFM(0.001) # print "CFM: %s ERP: %s"%(CFM, ERP) m_id = 0 self.pub_marker.eraseMarkers(0,3000, frame_id='world') # # Init Openrave # parser = OptionParser(description='Openrave Velma interface') OpenRAVEGlobalArguments.addOptions(parser) (options, leftargs) = parser.parse_args() self.env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) # self.openrave_robot = self.env.ReadRobotXMLFile('robots/barretthand_ros.robot.xml') mimic_joints = [ ("right_HandFingerTwoKnuckleOneJoint", "right_HandFingerOneKnuckleOneJoint*1.0", "|right_HandFingerOneKnuckleOneJoint 1.0", ""), ("right_HandFingerOneKnuckleThreeJoint", "right_HandFingerOneKnuckleTwoJoint*0.33333", "|right_HandFingerOneKnuckleTwoJoint 0.33333", ""), ("right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerTwoKnuckleTwoJoint*0.33333", "|right_HandFingerTwoKnuckleTwoJoint 0.33333", ""), ("right_HandFingerThreeKnuckleThreeJoint", "right_HandFingerThreeKnuckleTwoJoint*0.33333", "|right_HandFingerThreeKnuckleTwoJoint 0.33333", ""), ] rospack = rospkg.RosPack() self.urdf_module = RaveCreateModule(self.env, 'urdf') xacro_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.urdf.xml' urdf_uri = '/tmp/barrett_hand.urdf' srdf_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.srdf' arg1 = "collision_model_full:=true" arg2 = "collision_model_simplified:=false" arg3 = "collision_model_enlargement:=0.0" arg4 = "collision_model_no_hands:=false" subprocess.call(["rosrun", "xacro", "xacro", "-o", urdf_uri, xacro_uri, arg1, arg2, arg3, arg4]) robot_name = self.urdf_module.SendCommand('load ' + urdf_uri + ' ' + srdf_uri ) print "robot name: " + robot_name self.openrave_robot = self.env.GetRobot(robot_name) self.env.Remove(self.openrave_robot) for mimic in mimic_joints: mj = self.openrave_robot.GetJoint(mimic[0]) mj.SetMimicEquations(0, mimic[1], mimic[2], mimic[3]) self.openrave_robot.GetActiveManipulator().SetChuckingDirection([0,0,0,0])#directions) self.env.Add(self.openrave_robot) self.openrave_robot = self.env.GetRobot(robot_name) # print "robots: " # print self.env.GetRobots() joint_names = [] print "active joints:" for j in self.openrave_robot.GetJoints(): joint_names.append(j.GetName()) print j print "passive joints:" for j in self.openrave_robot.GetPassiveJoints(): joint_names.append(j.GetName()) print j # ODE does not support distance measure self.env.GetCollisionChecker().SetCollisionOptions(CollisionOptions.Contacts) self.env.Add(self.openrave_robot) vertices, faces = surfaceutils.readStl(rospack.get_path('velma_scripts') + "/data/meshes/klucz_gerda_ascii.stl", scale=1.0) # vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0) self.addTrimesh("object", vertices, faces) # self.addBox("object", 0.2,0.06,0.06) # self.addSphere("object", 0.15) # vertices, faces = self.getMesh("object") # # definition of the expected external wrenches # ext_wrenches = [] # origin of the external wrench (the end point of the key) wr_orig = PyKDL.Vector(0.039, 0.0, 0.0) for i in range(8): # expected force at the end point force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(0,1,0) # expected torque at the com torque = wr_orig * force ext_wrenches.append(PyKDL.Wrench(force, torque)) # expected force at the end point force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(-1,1,0) # expected torque at the com torque = wr_orig * force ext_wrenches.append(PyKDL.Wrench(force, torque)) # # definition of the grasps # grasp_id = 0 if grasp_id == 0: grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [60.0/180.0*math.pi, None, None, None] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.122103662206, -0.124395758838, -0.702726011729, 0.689777190329), PyKDL.Vector(-0.00115787237883, -0.0194999426603, 0.458197712898)) # self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.174202588426, -0.177472708083, -0.691231954612, 0.678495061771), PyKDL.Vector(0.0, -0.0213436260819, 0.459123969078)) elif grasp_id == 1: grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387771868, -0.708157209758, -0.0317875569224, 0.705090018033), PyKDL.Vector(4.65661287308e-10, 0.00145332887769, 0.472836345434)) elif grasp_id == 2: grasp_direction = [0, 1, 0, 0] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, 90.0/180.0*math.pi, 0] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387763947, -0.708157179826, -0.0317875555789, 0.705089928626), PyKDL.Vector(0.0143095180392, 0.00145332887769, 0.483659058809)) elif grasp_id == 3: grasp_direction = [0, 1, 1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00518634245761, -0.706548316769, -0.0182458505507, 0.707410947861), PyKDL.Vector(0.000126354629174, -0.00217361748219, 0.47637796402)) elif grasp_id == 4: grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, 100.0/180.0*math.pi, None, 100.0/180.0*math.pi] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.153445252933, -0.161230275653, 0.681741576082, 0.696913201022), PyKDL.Vector(0.000126355327666, 0.00152841210365, 0.466048002243)) elif grasp_id == 5: grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2 grasp_initial_configuration = [100.0/180.0*math.pi, 101.5/180.0*math.pi, None, 101.5/180.0*math.pi] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.155488650062, -0.159260521271, 0.690572597636, 0.688163302213), PyKDL.Vector(-0.000278688268736, 0.00575117766857, 0.461560428143)) elif grasp_id == 6: grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, 0, 0] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.512641041738, -0.485843507183, -0.514213889193, 0.48655882699), PyKDL.Vector(-0.000278423947748, -0.00292747467756, 0.445628076792)) else: print "ERROR: unknown grasp_id: %s"%(grasp_id) exit(0) self.updatePose("object", self.T_W_O) with open('barret_hand_openrave2ros_joint_map2.txt', 'r') as f: lines = f.readlines() joint_map = {} for line in lines: line_s = line.split() if len(line_s) == 2: joint_map[line_s[0]] = line_s[1] elif len(line_s) != 1: print "error in joint map file" exit(0) print joint_map self.pub_js = rospy.Publisher("/joint_states", JointState) if True: def getDirectionIndex(n): min_angle = -45.0/180.0*math.pi angle_range = 90.0/180.0*math.pi angles_count = 5 angles_count2 = angles_count * angles_count max_indices = angles_count2 * 6 if abs(n.x()) > abs(n.y()) and abs(n.x()) > abs(n.z()): if n.x() > 0: sec = 0 a1 = math.atan2(n.y(), n.x()) a2 = math.atan2(n.z(), n.x()) else: sec = 1 a1 = math.atan2(n.y(), -n.x()) a2 = math.atan2(n.z(), -n.x()) elif abs(n.y()) > abs(n.x()) and abs(n.y()) > abs(n.z()): if n.y() > 0: sec = 2 a1 = math.atan2(n.x(), n.y()) a2 = math.atan2(n.z(), n.y()) else: sec = 3 a1 = math.atan2(n.x(), -n.y()) a2 = math.atan2(n.z(), -n.y()) else: if n.z() > 0: sec = 4 a1 = math.atan2(n.x(), n.z()) a2 = math.atan2(n.y(), n.z()) else: sec = 5 a1 = math.atan2(n.x(), -n.z()) a2 = math.atan2(n.y(), -n.z()) a1i = int(angles_count*(a1-min_angle)/angle_range) a2i = int(angles_count*(a2-min_angle)/angle_range) if a1i < 0: print sec, a1i, a2i a1i = 0 if a1i >= angles_count: # print sec, a1i, a2i a1i = angles_count-1 if a2i < 0: print sec, a1i, a2i a2i = 0 if a2i >= angles_count: print sec, a1i, a2i a2i = angles_count-1 return sec * angles_count2 + a1i * angles_count + a2i # generate a dictionary of indexed directions normals_sphere_indexed_dir = velmautils.generateNormalsSphere(3.0/180.0*math.pi) print len(normals_sphere_indexed_dir) indices_directions = {} for n in normals_sphere_indexed_dir: index = getDirectionIndex(n) if not index in indices_directions: indices_directions[index] = [n] else: indices_directions[index].append(n) for index in indices_directions: n_mean = PyKDL.Vector() for n in indices_directions[index]: n_mean += n n_mean.Normalize() indices_directions[index] = n_mean # test direction indexing if False: normals_sphere = velmautils.generateNormalsSphere(3.0/180.0*math.pi) print len(normals_sphere) m_id = 0 for current_index in range(5*5*6): count = 0 r = random.random() g = random.random() b = random.random() for n in normals_sphere: sec = -1 a1 = None a2 = None index = getDirectionIndex(n) if index == current_index: m_id = self.pub_marker.publishSinglePointMarker(n*0.1, m_id, r=r, g=g, b=b, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.007, 0.007, 0.007), T=None) count += 1 print current_index, count for index in indices_directions: m_id = self.pub_marker.publishSinglePointMarker(indices_directions[index]*0.12, m_id, r=1, g=1, b=1, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.012, 0.012, 0.012), T=None) exit(0) # # PyODE test # if True: fixed_joints_names_for_fixed_DOF = [ ["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread ["right_HandFingerOneKnuckleTwoJoint", "right_HandFingerOneKnuckleThreeJoint"], # f1 ["right_HandFingerThreeKnuckleTwoJoint", "right_HandFingerThreeKnuckleThreeJoint"], # f3 ["right_HandFingerTwoKnuckleTwoJoint", "right_HandFingerTwoKnuckleThreeJoint"], # f2 ] coupled_joint_names_for_fingers = ["right_HandFingerOneKnuckleThreeJoint", "right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerThreeKnuckleThreeJoint"] actuated_joints_for_DOF = [ ["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread ["right_HandFingerOneKnuckleTwoJoint"], # f1 ["right_HandFingerThreeKnuckleTwoJoint"], # f3 ["right_HandFingerTwoKnuckleTwoJoint"]] # f2 ignored_links = ["world", "world_link"] self.run_ode_simulation = False self.insert6DofGlobalMarker(self.T_W_O) print "grasp_direction = [%s, %s, %s, %s] # spread, f1, f3, f2"%(grasp_direction[0], grasp_direction[1], grasp_direction[2], grasp_direction[3]) print "grasp_initial_configuration = [%s, %s, %s, %s]"%(grasp_initial_configuration[0], grasp_initial_configuration[1], grasp_initial_configuration[2], grasp_initial_configuration[3]) print "grasp_final_configuration = %s"%(self.openrave_robot.GetDOFValues()) rot_q = self.T_W_O.M.GetQuaternion() print "self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s, %s, %s, %s), PyKDL.Vector(%s, %s, %s))"%(rot_q[0], rot_q[1], rot_q[2], rot_q[3], self.T_W_O.p.x(), self.T_W_O.p.y(), self.T_W_O.p.z()) self.erase6DofMarker() raw_input("Press ENTER to continue...") # reset the gripper in Openrave self.openrave_robot.SetDOFValues([0,0,0,0]) # read the position of all links grasp_links_poses = {} for link in self.openrave_robot.GetLinks(): grasp_links_poses[link.GetName()] = self.OpenraveToKDL(link.GetTransform()) # print "obtained the gripper configuration for the grasp:" # print finalconfig[0] # # simulation in ODE # # Create a world object world = ode.World() # world.setGravity( (0,0,-3.81) ) world.setGravity( (0,0,0) ) CFM = world.getCFM() ERP = world.getERP() print "CFM: %s ERP: %s"%(CFM, ERP) # world.setCFM(0.001) # print "CFM: %s ERP: %s"%(CFM, ERP) self.space = ode.Space() # Create a body inside the world body = ode.Body(world) M = ode.Mass() M.setCylinderTotal(0.02, 1, 0.005, 0.09) body.setMass(M) ode_mesh = ode.TriMeshData() ode_mesh.build(vertices, faces) geom = ode.GeomTriMesh(ode_mesh, self.space) geom.name = "object" geom.setBody(body) self.setOdeBodyPose(geom, self.T_W_O) ode_gripper_geoms = {} for link in self.openrave_robot.GetLinks(): link_name = link.GetName() if link_name in ignored_links: print "ignoring: %s"%(link_name) continue print "adding: %s"%(link_name) ode_mesh_link = None body_link = None T_W_L = self.OpenraveToKDL(link.GetTransform()) col = link.GetCollisionData() vertices = col.vertices faces = col.indices ode_mesh_link = ode.TriMeshData() ode_mesh_link.build(vertices, faces) ode_gripper_geoms[link_name] = ode.GeomTriMesh(ode_mesh_link, self.space) if True: body_link = ode.Body(world) M_link = ode.Mass() M_link.setCylinderTotal(0.05, 1, 0.01, 0.09) body_link.setMass(M_link) ode_gripper_geoms[link_name].setBody(body_link) ode_gripper_geoms[link_name].name = link.GetName() self.setOdeBodyPose(body_link, T_W_L) actuated_joint_names = [] for dof in range(4): for joint_name in actuated_joints_for_DOF[dof]: actuated_joint_names.append(joint_name) ode_gripper_joints = {} for joint_name in joint_names: joint = self.openrave_robot.GetJoint(joint_name) link_parent = joint.GetHierarchyParentLink().GetName() link_child = joint.GetHierarchyChildLink().GetName() if link_parent in ode_gripper_geoms and link_child in ode_gripper_geoms: axis = joint.GetAxis() anchor = joint.GetAnchor() if joint_name in actuated_joint_names: ode_gripper_joints[joint_name] = ode.AMotor(world) ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody()) ode_gripper_joints[joint_name].setMode(ode.AMotorUser)#Euler) # ode_gripper_joints[joint_name].setNumAxes(3) ode_gripper_joints[joint_name].setAxis(0,1,axis) # ode_gripper_joints[joint_name].setAxis(1,1,axis) # ode_gripper_joints[joint_name].setAxis(2,1,axis) ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0) # ode_gripper_joints[joint_name].setParam(ode.ParamFMax2, 10.0) # ode_gripper_joints[joint_name].setParam(ode.ParamFMax3, 10.0) else: ode_gripper_joints[joint_name] = ode.HingeJoint(world) ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody()) ode_gripper_joints[joint_name].setAxis(-axis) ode_gripper_joints[joint_name].setAnchor(anchor) # ode_gripper_joints[joint_name] = ode.HingeJoint(world) # ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody()) # ode_gripper_joints[joint_name].setAxis(-axis) # ode_gripper_joints[joint_name].setAnchor(anchor) # if joint_name in actuated_joint_names: # ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0) limits = joint.GetLimits() value = joint.GetValue(0) # print joint_name # print "limits: %s %s"%(limits[0], limits[1]) # print "axis: %s"%(axis) # print "anchor: %s"%(anchor) # print "value: %s"%(value) lim = [limits[0], limits[1]] if limits[0] <= -math.pi: # print "lower joint limit %s <= -PI, setting to -PI+0.01"%(limits[0]) lim[0] = -math.pi + 0.01 if limits[1] >= math.pi: # print "upper joint limit %s >= PI, setting to PI-0.01"%(limits[1]) lim[1] = math.pi - 0.01 ode_gripper_joints[joint_name].setParam(ode.ParamLoStop, lim[0]) ode_gripper_joints[joint_name].setParam(ode.ParamHiStop, lim[1]) # ode_gripper_joints[joint_name].setParam(ode.ParamFudgeFactor, 0.5) ode_fixed_joint = ode.FixedJoint(world) ode_fixed_joint.attach(None, ode_gripper_geoms["right_HandPalmLink"].getBody()) ode_fixed_joint.setFixed() # # set the poses of all links as for the grasp # for link_name in grasp_links_poses: if link_name in ignored_links: continue ode_body = ode_gripper_geoms[link_name].getBody() T_W_L = grasp_links_poses[link_name] self.setOdeBodyPose(ode_body, T_W_L) fixed_joint_names = [] fixed_joint_names += coupled_joint_names_for_fingers for dof in range(4): if grasp_direction[dof] == 0.0: for joint_name in fixed_joints_names_for_fixed_DOF[dof]: if not joint_name in fixed_joint_names: fixed_joint_names.append(joint_name) # # change all coupled joints to fixed joints # fixed_joints = {} for joint_name in fixed_joint_names: # save the bodies attached body0 = ode_gripper_joints[joint_name].getBody(0) body1 = ode_gripper_joints[joint_name].getBody(1) # save the joint angle if joint_name in actuated_joint_names: angle = ode_gripper_joints[joint_name].getAngle(0) else: angle = ode_gripper_joints[joint_name].getAngle() # detach the joint ode_gripper_joints[joint_name].attach(None, None) del ode_gripper_joints[joint_name] fixed_joints[joint_name] = [ode.FixedJoint(world), angle] fixed_joints[joint_name][0].attach(body0, body1) fixed_joints[joint_name][0].setFixed() # change all actuated joints to motor joints # actuated_joint_names = [] # for dof in range(4): # for joint_name in actuated_joints_for_DOF[dof]: # actuated_joint_names.append(joint_name) # for joint_name in actuated_joint_names: # A joint group for the contact joints that are generated whenever # two bodies collide contactgroup = ode.JointGroup() print "ode_gripper_geoms:" print ode_gripper_geoms initial_T_W_O = self.T_W_O # Do the simulation... dt = 0.001 total_time = 0.0 failure = False # # PID # Kc = 1.0 # Ti = 1000000000000.0 KcTi = 0.0 Td = 0.0 Ts = 0.001 pos_d = 1.0 e0 = 0.0 e1 = 0.0 e2 = 0.0 u = 0.0 u_max = 1.0 while total_time < 5.0 and not rospy.is_shutdown(): # # ODE simulation # joint = ode_gripper_joints["right_HandFingerOneKnuckleTwoJoint"] e2 = e1 e1 = e0 e0 = pos_d - joint.getAngle(0) u = u + Kc * (e0 - e1) + KcTi * Ts * e0 + Kc * Td / Ts * (e0 - 2.0 * e1 + e2) if u > u_max: u = u_max if u < -u_max: u = -u_max joint.setParam(ode.ParamFMax, 10000.0) joint.setParam(ode.ParamVel, 1.1) # joint.setParam(ode.ParamVel2, 1.1) # joint.setParam(ode.ParamVel3, 1.1) print joint.getAngle(0), " ", u #, " ", joint.getAngleRate(0) # print u # joint.addTorque(u) # for dof in range(4): # for joint_name in actuated_joints_for_DOF[dof]: # if joint_name in ode_gripper_joints: # ode_gripper_joints[joint_name].addTorque(1*grasp_direction[dof]) self.grasp_contacts = [] self.space.collide((world,contactgroup), self.near_callback) world.step(dt) total_time += dt contactgroup.empty() # # ROS interface # old_m_id = m_id m_id = 0 # publish frames from ODE if False: for link_name in ode_gripper_geoms: link_body = ode_gripper_geoms[link_name].getBody() if link_body == None: link_body = ode_gripper_geoms[link_name] T_W_Lsim = self.getOdeBodyPose(link_body) m_id = self.pub_marker.publishFrameMarker(T_W_Lsim, m_id, scale=0.05, frame='world', namespace='default') # publish the mesh of the object T_W_Osim = self.getOdeBodyPose(body) m_id = self.pub_marker.publishConstantMeshMarker("package://velma_scripts/data/meshes/klucz_gerda_binary.stl", m_id, r=1, g=0, b=0, scale=1.0, frame_id='world', namespace='default', T=T_W_Osim) # update the gripper visualization in ros js = JointState() js.header.stamp = rospy.Time.now() for jn in joint_map: ros_joint_name = joint_map[jn] js.name.append(ros_joint_name) if jn in ode_gripper_joints: if jn in actuated_joint_names: js.position.append( ode_gripper_joints[jn].getAngle(0) ) else: js.position.append( ode_gripper_joints[jn].getAngle() ) elif jn in fixed_joints: js.position.append(fixed_joints[jn][1]) else: js.position.append(0) self.pub_js.publish(js) # draw contacts for c in self.grasp_contacts: cc = PyKDL.Vector(c[0], c[1], c[2]) cn = PyKDL.Vector(c[3], c[4], c[5]) m_id = self.pub_marker.publishVectorMarker(cc, cc+cn*0.04, m_id, 1, 0, 0, frame='world', namespace='default', scale=0.003) if m_id < old_m_id: self.pub_marker.eraseMarkers(m_id,old_m_id+1, frame_id='world') diff_T_W_O = PyKDL.diff(initial_T_W_O, T_W_Osim) # if diff_T_W_O.vel.Norm() > 0.02 or diff_T_W_O.rot.Norm() > 30.0/180.0*math.pi: # print "the object moved" # print diff_T_W_O # failure = True # break rospy.sleep(0.01) if not failure: T_O_Wsim = T_W_Osim.Inverse() TR_O_Wsim = PyKDL.Frame(T_O_Wsim.M) contacts = [] for c in self.grasp_contacts: cc_W = PyKDL.Vector(c[0], c[1], c[2]) cn_W = PyKDL.Vector(c[3], c[4], c[5]) cc_O = T_O_Wsim * cc_W cn_O = TR_O_Wsim * cn_W contacts.append([cc_O[0], cc_O[1], cc_O[2], cn_O[0], cn_O[1], cn_O[2]]) gws, contact_planes = self.generateGWS(contacts, 1.0) grasp_quality = None for wr in ext_wrenches: wr_qual = self.getQualityMeasure2(gws, wr) if grasp_quality == None or wr_qual < grasp_quality: grasp_quality = wr_qual grasp_quality_classic = self.getQualityMeasure(gws) print "grasp_quality_classic: %s grasp_quality: %s"%(grasp_quality_classic, grasp_quality) exit(0)
def spin(self): rospack = rospkg.RosPack() urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.urdf' srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.srdf' dyn_model = planar5.DynModelPlanar5() col = collision_model.CollisionModel() col.readUrdfSrdf(urdf_path, srdf_path) self.joint_names = ["0_joint", "1_joint", "2_joint", "3_joint", "4_joint"] effector_name = 'effector' ndof = len(self.joint_names) # robot state self.q = np.zeros( ndof ) self.dq = np.zeros( ndof ) self.q[0] = 0.2 self.q[1] = -0.1 self.q[2] = -0.1 self.q[3] = -0.1 self.q[4] = -0.1 solver = fk_ik.FkIkSolver(self.joint_names, [], None) self.publishJointState() # obstacles obst = [] obj = collision_model.CollisionModel.Collision() obj.type = "capsule" obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.5, 0.7, 0)) obj.radius = 0.1 obj.length = 0.4 obst.append( obj ) obj = collision_model.CollisionModel.Collision() obj.type = "capsule" obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.2, -0.9, 0)) obj.radius = 0.1 obj.length = 0.4 obst.append( obj ) obj = collision_model.CollisionModel.Collision() obj.type = "sphere" obj.T_L_O = PyKDL.Frame(PyKDL.Vector(1, -0.2, 0)) obj.radius = 0.05 obst.append( obj ) last_time = rospy.Time.now() limit_range = np.zeros( ndof ) max_trq = np.zeros( ndof ) for q_idx in range( ndof ): limit_range[q_idx] = 15.0/180.0*math.pi max_trq[q_idx] = 10.0 dyn_model.computeM(self.q) obst_offset = 0.0 counter = 10000 while not rospy.is_shutdown(): obst_offset += 0.0001 obst[-1].T_L_O = PyKDL.Frame(PyKDL.Vector(1, -0.2+math.sin(obst_offset), 0)) if counter > 800: r_HAND_target = PyKDL.Frame(PyKDL.Rotation.RotZ(random.uniform(-math.pi, math.pi)), PyKDL.Vector(random.uniform(0,2), random.uniform(-1,1), 0)) qt = r_HAND_target.M.GetQuaternion() pt = r_HAND_target.p print "**** STATE ****" print "r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s), PyKDL.Vector(%s,%s,%s))"%(qt[0],qt[1],qt[2],qt[3],pt[0],pt[1],pt[2]) print "self.q = np.array(", self.q, ")" print "self.dq = np.array(", self.dq, ")" counter = 0 counter += 1 time_elapsed = rospy.Time.now() - last_time # # mass matrix # dyn_model.computeM(self.q) M = dyn_model.M Minv = dyn_model.Minv # # JLC # torque_JLC = np.zeros( ndof ) K = np.zeros( (ndof, ndof) ) for q_idx in range( ndof ): torque_JLC[q_idx] = self.jointLimitTrq(solver.lim_upper[q_idx], solver.lim_lower[q_idx], limit_range[q_idx], max_trq[q_idx], self.q[q_idx]) if abs(torque_JLC[q_idx]) > 0.001: K[q_idx,q_idx] = max_trq[q_idx]/limit_range[q_idx] else: K[q_idx,q_idx] = 0.001 w, v = scipy.linalg.eigh(a=K, b=M) # q_ = dyn_model.gaussjordan(np.matrix(v)) q_ = np.linalg.inv( np.matrix(v) ) k0_ = w k0_sqrt = np.zeros( k0_.shape ) for q_idx in range( ndof ): k0_sqrt[q_idx] = math.sqrt(k0_[q_idx]) tmpNN_ = np.diag(k0_sqrt) D = 2.0 * q_.H * 0.7 * tmpNN_ * q_ torque_JLC_mx = -D * np.matrix(self.dq).transpose() for q_idx in range( ndof ): torque_JLC[q_idx] += torque_JLC_mx[q_idx, 0] # calculate jacobian (the activation function) J_JLC = np.matrix(numpy.zeros( (ndof, ndof) )) for q_idx in range( ndof ): if self.q[q_idx] < solver.lim_lower_soft[q_idx]: J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_lower_soft[q_idx]) / abs(solver.lim_lower[q_idx] - solver.lim_lower_soft[q_idx])) elif self.q[q_idx] > solver.lim_upper_soft[q_idx]: J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_upper_soft[q_idx]) / abs(solver.lim_upper[q_idx] - solver.lim_upper_soft[q_idx])) else: J_JLC[q_idx,q_idx] = 0.0 N_JLC = np.matrix(np.identity( ndof )) - (J_JLC.transpose() * J_JLC) links_fk = {} for link in col.links: links_fk[link.name] = solver.calculateFk('base', link.name, self.q) # # collision constraints # link_collision_map = {} if True: activation_dist = 0.05 # self collision total_contacts = 0 for link1_name, link2_name in col.collision_pairs: link1 = col.link_map[link1_name] T_B_L1 = links_fk[link1_name] link2 = col.link_map[link2_name] T_B_L2 = links_fk[link2_name] for col1 in link1.col: for col2 in link2.col: T_B_C1 = T_B_L1 * col1.T_L_O T_B_C2 = T_B_L2 * col2.T_L_O c_dist = (T_B_C1 * PyKDL.Vector() - T_B_C2 * PyKDL.Vector()).Norm() if col1.type == "capsule": c_dist -= col1.radius + col1.length/2 elif col1.type == "sphere": c_dist -= col1.radius if col2.type == "capsule": c_dist -= col2.radius + col2.length/2 elif col2.type == "sphere": c_dist -= col2.radius if c_dist > activation_dist: continue dist = None if col1.type == "capsule" and col2.type == "capsule": line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distanceLines(line1, line2) elif col1.type == "capsule" and col2.type == "sphere": line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) pt = T_B_C2 * PyKDL.Vector() dist, p1_B, p2_B = self.distanceLinePoint(line, pt) elif col1.type == "sphere" and col2.type == "capsule": pt = T_B_C1 * PyKDL.Vector() line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distancePointLine(pt, line) elif col1.type == "sphere" and col2.type == "sphere": dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector()) else: print "ERROR: unknown collision type:", col1.type, col2.type exit(0) if dist != None: dist -= col1.radius + col2.radius v = p2_B - p1_B v.Normalize() n1_B = v n2_B = -v p1_B += n1_B * col1.radius p2_B += n2_B * col2.radius if dist < activation_dist: if not (link1_name, link2_name) in link_collision_map: link_collision_map[(link1_name, link2_name)] = [] link_collision_map[(link1_name, link2_name)].append( (p1_B, p2_B, dist, n1_B, n2_B) ) # environment collisions for link in col.links: if link.col == None: continue link1_name = link.name T_B_L1 = links_fk[link1_name] T_B_L2 = links_fk["base"] for col1 in link.col: for col2 in obst: T_B_C1 = T_B_L1 * col1.T_L_O T_B_C2 = T_B_L2 * col2.T_L_O dist = None if col1.type == "capsule" and col2.type == "capsule": line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distanceLines(line1, line2) elif col1.type == "capsule" and col2.type == "sphere": line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) pt = T_B_C2 * PyKDL.Vector() dist, p1_B, p2_B = self.distanceLinePoint(line, pt) elif col1.type == "sphere" and col2.type == "capsule": pt = T_B_C1 * PyKDL.Vector() line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distancePointLine(pt, line) elif col1.type == "sphere" and col2.type == "sphere": dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector()) else: print "ERROR: unknown collision type:", col1.type, col2.type exit(0) if dist != None: dist -= col1.radius + col2.radius v = p2_B - p1_B v.Normalize() n1_B = v n2_B = -v p1_B += n1_B * col1.radius p2_B += n2_B * col2.radius if dist < activation_dist: if not (link1_name, "base") in link_collision_map: link_collision_map[(link1_name, "base")] = [] link_collision_map[(link1_name, "base")].append( (p1_B, p2_B, dist, n1_B, n2_B) ) torque_col = np.matrix(np.zeros( (ndof,1) )) Ncol = np.matrix(np.identity( ndof )) m_id = 0 for link1_name, link2_name in link_collision_map: for (p1_B, p2_B, dist, n1_B, n2_B) in link_collision_map[ (link1_name, link2_name) ]: if dist > 0.0: m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 1, 1, frame='base', namespace='default', scale=0.02) else: m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 0, 0, frame='base', namespace='default', scale=0.02) T_B_L1 = links_fk[link1_name] T_L1_B = T_B_L1.Inverse() T_B_L2 = links_fk[link2_name] T_L2_B = T_B_L2.Inverse() p1_L1 = T_L1_B * p1_B p2_L2 = T_L2_B * p2_B n1_L1 = PyKDL.Frame(T_L1_B.M) * n1_B n2_L2 = PyKDL.Frame(T_L2_B.M) * n2_B # print p1_L1, p1_L1+n1_L1*0.2 # print p2_L2, p2_L2+n2_L2*0.2 # m_id = self.pub_marker.publishVectorMarker(p1_L1, p1_L1+n1_L1*0.2, m_id, 0, 1, 0, frame=link1_name, namespace='default', scale=0.02) # m_id = self.pub_marker.publishVectorMarker(T_B_L2*p2_L2, T_B_L2*(p2_L2+n2_L2*0.2), m_id, 0, 0, 1, frame="base", namespace='default', scale=0.02) jac1 = PyKDL.Jacobian( ndof ) jac2 = PyKDL.Jacobian( ndof ) common_link_name = solver.getJacobiansForPairX(jac1, jac2, link1_name, p1_L1, link2_name, p2_L2, self.q, None) # print link1_name, link2_name depth = (activation_dist - dist) # repulsive force Fmax = 20.0 if dist <= activation_dist: f = (dist - activation_dist) / activation_dist else: f = 0.0 Frep = Fmax * f * f K = 2.0 * Fmax / (activation_dist * activation_dist) # the mapping between motions along contact normal and the Cartesian coordinates e1 = n1_L1 e2 = n2_L2 Jd1 = np.matrix([e1[0], e1[1], e1[2]]) Jd2 = np.matrix([e2[0], e2[1], e2[2]]) # rewrite the linear part of the jacobian jac1_mx = np.matrix(np.zeros( (3, ndof) )) jac2_mx = np.matrix(np.zeros( (3, ndof) )) for q_idx in range(ndof): col1 = jac1.getColumn(q_idx) col2 = jac2.getColumn(q_idx) for row_idx in range(3): jac1_mx[row_idx, q_idx] = col1[row_idx] jac2_mx[row_idx, q_idx] = col2[row_idx] Jcol1 = Jd1 * jac1_mx Jcol2 = Jd2 * jac2_mx Jcol = np.matrix(np.zeros( (1, ndof) )) for q_idx in range(ndof): Jcol[0, q_idx] = Jcol1[0, q_idx] + Jcol2[0, q_idx] # calculate relative velocity between points ddij = Jcol * np.matrix(self.dq).transpose() activation = min(1.0, 5.0*depth/activation_dist) activation = max(0.0, activation) if ddij <= 0.0: activation = 0.0 a_des = activation # print "activation", activation # raw_input(".") if rospy.is_shutdown(): exit(0) Ncol12 = np.matrix(np.identity(ndof)) - (Jcol.transpose() * a_des * Jcol) Ncol = Ncol * Ncol12 # calculate collision mass Mdij = Jcol * Minv * Jcol.transpose() D = 2.0 * 0.7 * math.sqrt(Mdij * K) d_torque = Jcol.transpose() * (-Frep - D * ddij) torque_col += d_torque self.pub_marker.eraseMarkers(m_id, 10, frame_id='base', namespace='default') # # end-effector task # T_B_E = links_fk[effector_name] r_HAND_current = T_B_E diff = PyKDL.diff(r_HAND_current, r_HAND_target) r_HAND_diff = np.array( [diff[0], diff[1], diff[5]] ) Kc = np.array( [10, 10, 1] ) Dxi = np.array( [0.7, 0.7, 0.7] ) wrench = np.zeros( 3 ) for dim in range(3): wrench[dim] = Kc[dim] * r_HAND_diff[dim] J_r_HAND_6 = solver.getJacobian('base', effector_name, self.q) J_r_HAND = np.matrix( np.zeros( (3,5) ) ) for q_idx in range( ndof ): J_r_HAND[0, q_idx] = J_r_HAND_6[0, q_idx] J_r_HAND[1, q_idx] = J_r_HAND_6[1, q_idx] J_r_HAND[2, q_idx] = J_r_HAND_6[5, q_idx] torque_HAND = J_r_HAND.transpose() * np.matrix(wrench).transpose() A = J_r_HAND * Minv * J_r_HAND.transpose() # A = dyn_model.gaussjordan(A) A = np.linalg.inv(A) tmpKK_ = np.matrix(np.diag(Kc)) w, v = scipy.linalg.eigh(a=tmpKK_, b=A) # Q = dyn_model.gaussjordan(np.matrix(v)) Q = np.linalg.inv( np.matrix(v) ) K0 = w Dc = Q.transpose() * np.matrix( np.diag(Dxi) ) K0_sqrt = np.zeros( K0.shape ) for dim_idx in range( 3 ): K0_sqrt[dim_idx] = math.sqrt(K0[dim_idx]) Dc = 2.0 * Dc * np.matrix( np.diag(K0_sqrt) ) * Q F = Dc * J_r_HAND * np.matrix(self.dq).transpose() torque_HAND_mx = -J_r_HAND.transpose() * F for q_idx in range( ndof ): torque_HAND[q_idx] += torque_HAND_mx[q_idx, 0] # null-space # tmpNK_.noalias() = J * Mi; # tmpKK_.noalias() = tmpNK_ * JT; # luKK_.compute(tmpKK_); # tmpKK_ = luKK_.inverse(); # tmpKN_.noalias() = Mi * JT; # Ji.noalias() = tmpKN_ * tmpKK_; # # P.noalias() = Eigen::MatrixXd::Identity(P.rows(), P.cols()); # P.noalias() -= J.transpose() * A * J * Mi; torque = np.matrix(torque_JLC).transpose() + N_JLC.transpose() * (torque_col + (Ncol.transpose() * torque_HAND)) # torque = torque_HAND time_d = 0.01 # simulate one step ddq = dyn_model.accel(self.q, self.dq, torque) for q_idx in range(ndof): self.dq[q_idx] += ddq[q_idx,0] * time_d self.q[q_idx] += self.dq[q_idx] * time_d if time_elapsed.to_sec() > 0.05: m_id = 0 m_id = self.publishRobotModelVis(m_id, col, links_fk) m_id = self.publishObstaclesVis(m_id, obst) last_time = rospy.Time.now() self.publishJointState() self.publishTransform(r_HAND_target, "hand_dest")
T_C_M = [] T_T2_7 = [] T_C_M_stable = PyKDL.Frame() T_T2_7_stable = PyKDL.Frame() stable_t = 0 while True: rospy.sleep(0.1) try: pose = tf_listener.lookupTransform('camera', 'ar_marker_'+str(right_makrer_id), rospy.Time(0)) T_C_M_current = pm.fromTf(pose) pose = tf_listener.lookupTransform('head_tilt_link', 'right_arm_7_link', rospy.Time(0)) T_T2_7_current = pm.fromTf(pose) except: continue d1 = PyKDL.diff(T_C_M_stable, T_C_M_current) d2 = PyKDL.diff(T_T2_7_stable, T_T2_7_current) score = d2.vel.Norm() + d2.rot.Norm() if score > 0.002: stable_t = 0 T_C_M_stable = copy.deepcopy(T_C_M_current) T_T2_7_stable = copy.deepcopy(T_T2_7_current) else: stable_t += 1 add = True if stable_t > 10: for t in T_T2_7: d = PyKDL.diff(T_T2_7_current, t) if d.rot.Norm() < 0.1: add = False break
def poseUpdaterThread(self, args, *args2): index = 0 z_limit = 0.3 while not rospy.is_shutdown(): rospy.sleep(0.1) if self.allow_update_objects_pose == None or not self.allow_update_objects_pose: continue for obj_name in self.dyn_obj_markers: obj = self.dyn_obj_markers[obj_name] visible_markers_Br_Co = [] visible_markers_weights_ori = [] visible_markers_weights_pos = [] visible_markers_idx = [] for marker in obj:#.markers: T_Br_M = self.getMarkerPose(marker[0], wait = False, timeBack = 0.3) if T_Br_M != None and self.velma != None: T_B_C = self.getCameraPose() T_C_M = T_B_C.Inverse() * T_Br_M v = T_C_M * PyKDL.Vector(0,0,1) - T_C_M * PyKDL.Vector() if v.z() > -z_limit: continue # v.z() is in range (-1.0, -0.3) weight = ((-v.z()) - z_limit)/(1.0-z_limit) if weight > 1.0 or weight < 0.0: print "error: weight==%s"%(weight) T_Co_M = marker[1] T_Br_Co = T_Br_M * T_Co_M.Inverse() visible_markers_Br_Co.append(T_Br_Co) visible_markers_weights_ori.append(1.0-weight) visible_markers_weights_pos.append(weight) visible_markers_idx.append(marker[0]) if len(visible_markers_Br_Co) > 0: # if obj.name == "object": # print "vis: %s"%(visible_markers_idx) # print "w_o: %s"%(visible_markers_weights_ori) # print "w_p: %s"%(visible_markers_weights_pos) # first calculate mean pose without weights R_B_Co = velmautils.meanOrientation(visible_markers_Br_Co)[1] p_B_Co = velmautils.meanPosition(visible_markers_Br_Co) T_B_Co = PyKDL.Frame(copy.deepcopy(R_B_Co.M), copy.deepcopy(p_B_Co)) distances = [] for m_idx in range(0, len(visible_markers_Br_Co)): diff = PyKDL.diff(T_B_Co, visible_markers_Br_Co[m_idx]) distances.append( [diff, m_idx] ) Br_Co_list = [] weights_ori = [] weights_pos = [] for d in distances: if d[0].vel.Norm() > 0.04 or d[0].rot.Norm() > 15.0/180.0*math.pi: continue Br_Co_list.append( visible_markers_Br_Co[d[1]] ) weights_ori.append( visible_markers_weights_ori[d[1]] ) weights_pos.append( visible_markers_weights_pos[d[1]] ) if len(Br_Co_list) > 0: R_B_Co = velmautils.meanOrientation(Br_Co_list, weights=weights_ori)[1] p_B_Co = velmautils.meanPosition(Br_Co_list, weights=weights_pos) # obj.updatePose( PyKDL.Frame(copy.deepcopy(R_B_Co.M), copy.deepcopy(p_B_Co)) ) self.openrave.updatePose(obj_name, T_Br_Co) index += 1 if index >= 100: index = 0
def find_checkerboard_service(self, req): poses = [] min_x = self.ros_image.width min_y = self.ros_image.height max_x = 0 max_y = 0 for i in range(10): rospy.sleep(0.5) #copy the image over with self.mutex: image = self.ros_image result = self.find_checkerboard_pose(image, req.corners_x, req.corners_y, req.spacing_x, req.spacing_y, self.width_scaling, self.height_scaling) if result is None: rospy.logerr("Couldn't find a checkerboard") continue p, corners = result poses.append(p) for j in range(corners.cols): x = cv.GetReal2D(corners, 0, j) y = cv.GetReal2D(corners, 1, j) min_x = min(min_x, x) max_x = max(max_x, x) min_y = min(min_y, y) max_y = max(max_y, y) # convert all poses to twists twists = [] for p in poses: twists.append(PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose))) # get average twist twist_res = PyKDL.Twist() PyKDL.SetToZero(twist_res) for t in twists: for i in range(3): twist_res.vel[i] += t.vel[i]/len(poses) twist_res.rot[i] += t.rot[i]/len(poses) # get noise noise_rot = 0 noise_vel = 0 for t in twists: n_vel = (t.vel - twist_res.vel).Norm() n_rot = (t.rot - twist_res.rot).Norm() if n_vel > noise_vel: noise_vel = n_vel if n_rot > noise_rot: noise_rot = n_rot # set service resul pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0) pose_msg = PoseStamped() pose_msg.header = poses[0].header pose_msg.pose = posemath.toMsg(pose_res) return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y, max_y, noise_vel, noise_rot)
def my_diff(frame1, frame2, dt): return ( kdltwist_to_narray( PyKDL.diff( narray_to_kdlframe(frame1), narray_to_kdlframe(frame2), dt)))
def spin(self): simulation_only = False key_endpoint_T = PyKDL.Vector(0,0,0.2) T_B_P = PyKDL.Frame(PyKDL.Rotation(), PyKDL.Vector(1,0,1)) m_id = 0 self.pub_marker.eraseMarkers(0,3000, frame_id='world') print "creating interface for Velma..." # create the interface for Velma robot self.velma = Velma() print "done." m_id = 0 self.pub_marker.eraseMarkers(0,3000, frame_id='world') # key and grasp parameters self.T_E_O = PyKDL.Frame() self.T_E_Orot = PyKDL.Frame(PyKDL.Vector(0, 0, 0.07)) self.key_axis_O = PyKDL.Vector(0,0,1) self.key_up_O = PyKDL.Vector(1,0,0) self.key_side_O = self.key_axis_O * self.key_up_O # change the tool - the safe way print "switching to joint impedance..." if not self.velma.switchToJoint(): print "ERROR: switchToJoint" exit(0) rospy.sleep(0.5) print "updating tool..." self.velma.updateTransformations() self.velma.updateAndMoveToolOnly(PyKDL.Frame(self.velma.T_W_E.p+PyKDL.Vector(0.1,0,0)), 0.1) rospy.sleep(0.5) print "done." print "switching to cartesian impedance..." if not self.velma.switchToCart(): print "ERROR: switchToCart" exit(0) rospy.sleep(0.5) raw_input("Press ENTER to continue...") q_grasp = [1.4141768883517725, 1.4179811607057289, 1.4377081536379384, 0] q_pre = 10.0/180.0*math.pi hv = [1.2, 1.2, 1.2, 1.2] ht = [3000, 3000, 3000, 3000] # set gripper configuration if True: self.velma.moveHand([0.0, 0.0, 0.0, 0.0/180.0*math.pi], hv, ht, 5000, True) rospy.sleep(3) self.velma.moveHand([q_grasp[0]-q_pre, q_grasp[1]-q_pre, q_grasp[2]-q_pre, q_grasp[3]], hv, ht, 5000, True) rospy.sleep(5) self.velma.moveHand([q_grasp[0]+q_pre, q_grasp[1]+q_pre, q_grasp[2]+q_pre, q_grasp[3]], hv, ht, 5000, True) rospy.sleep(3) if False: # start with very low stiffness print "setting stiffness to very low value" k_low = Wrench(Vector3(1.0, 1.0, 1.0), Vector3(0.5, 0.5, 0.5)) self.velma.moveImpedance(k_low, 0.5) if self.velma.checkStopCondition(0.5): exit(0) print "done." print "identifying the parameters of tool in..." wait_time = 20 for i in range(wait_time): print "%s s"%(wait_time-i) rospy.sleep(1.0) print "collecting points..." self.collect_poses = True T_B_T_list = [] thread.start_new_thread(self.posesCollectorThread, (T_B_T_list,)) collect_time = 10 for i in range(collect_time): print "%s s"%(collect_time-i) rospy.sleep(1.0) self.collect_poses = False rospy.sleep(0.5) key_endpoint_T, error = self.estCommonPoint(T_B_T_list) print key_endpoint_T, error self.key_endpoint_O = self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() * self.velma.T_W_T * key_endpoint_T print "self.key_endpoint_O = PyKDL.Vector(%s, %s, %s)"%(self.key_endpoint_O[0], self.key_endpoint_O[1], self.key_endpoint_O[2]) print "done." else: # self.key_endpoint_O = PyKDL.Vector(-0.00248664992634, -6.7348683886e-05, 0.232163117525) self.key_endpoint_O = PyKDL.Vector(0.000256401261281, -0.000625166847342, 0.232297442735) k_high = Wrench(Vector3(1000.0, 1000.0, 1000.0), Vector3(150, 150, 150)) max_force = 12.0 max_torque = 12.0 path_tol = (PyKDL.Vector(max_force/k_high.force.x, max_force/k_high.force.y, max_force/k_high.force.z), PyKDL.Vector(max_torque/k_high.torque.x, max_torque/k_high.torque.y, max_torque/k_high.torque.z)) max_force2 = 20.0 max_torque2 = 20.0 path_tol2 = (PyKDL.Vector(max_force2/k_high.force.x, max_force2/k_high.force.y, max_force2/k_high.force.z), PyKDL.Vector(max_torque2/k_high.torque.x, max_torque2/k_high.torque.y, max_torque2/k_high.torque.z)) path_tol3 = (PyKDL.Vector(max_force/k_high.force.x, max_force/k_high.force.y, max_force2/k_high.force.z), PyKDL.Vector(max_torque/k_high.torque.x, max_torque/k_high.torque.y, max_torque/k_high.torque.z)) print "path tolerance:", path_tol # k_hole_in = Wrench(Vector3(1000.0, 500.0, 500.0), Vector3(200, 200, 200)) k_hole_in = Wrench(Vector3(100.0, 1000.0, 1000.0), Vector3(50, 5, 5)) path_tol_in = (PyKDL.Vector(max_force2/k_hole_in.force.x, max_force/k_hole_in.force.y, max_force/k_hole_in.force.z), PyKDL.Vector(max_torque2/k_hole_in.torque.x, max_torque2/k_hole_in.torque.y, max_torque2/k_hole_in.torque.z)) path_tol_in2 = (PyKDL.Vector(max_force2/k_hole_in.force.x, max_force2/k_hole_in.force.y, max_force2/k_hole_in.force.z), PyKDL.Vector(max_torque2/k_hole_in.torque.x, max_torque2/k_hole_in.torque.y, max_torque2/k_hole_in.torque.z)) if False: k_increase = [ Wrench(Vector3(10.0, 10.0, 10.0), Vector3(5, 5, 5)), Wrench(Vector3(50.0, 50.0, 50.0), Vector3(25, 25, 25)), Wrench(Vector3(250.0, 250.0, 250.0), Vector3(100, 100, 100)) ] for k in k_increase: raw_input("Press Enter to set bigger stiffness...") self.velma.updateTransformations() T_B_Wd = self.velma.T_B_W time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) print "moving the wrist to the current pose in " + str(time) + " s..." self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14))) rospy.sleep(time) print "done." print "setting stiffness to bigger value" self.velma.moveImpedance(k, 1.0) if self.velma.checkStopCondition(1.1): exit(0) print "done." print "move the grasped key near the key hole..." self.follow_wrist_pose = True thread.start_new_thread(self.wristFollowerThread, (None,)) raw_input("Press ENTER to save the pose...") self.follow_wrist_pose = False rospy.sleep(0.5) self.velma.updateTransformations() self.T_B_O_nearHole = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O p = self.T_B_O_nearHole.p q = self.T_B_O_nearHole.M.GetQuaternion() print "self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s, %s, %s, %s), PyKDL.Vector(%s, %s, %s))"%(q[0], q[1], q[2], q[3], p[0], p[1], p[2]) T_B_Wd = self.velma.T_B_W time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) print "moving the wrist to the current pose in " + str(time) + " s..." self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14))) status, feedback = self.velma.waitForCartEnd() print "setting stiffness to", k_high self.velma.moveImpedance(k_high, 1.0) if self.velma.checkStopCondition(1.0): exit(0) print "done." else: # self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.697525674378, -0.00510212356955, 0.715527762697, 0.0381041038336), PyKDL.Vector(0.528142123375, 0.0071159410235, 1.31300679435)) # self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.699716675653, -0.0454110538496, 0.709529999372, 0.0700113561626), PyKDL.Vector(0.546491646893, -0.101297899801, 1.30959887442)) self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.71891504857, -0.0529880479354, 0.691118088949, 0.0520500417212), PyKDL.Vector(0.883081316461, -0.100813768303, 0.95381559114)) k_increase = [ Wrench(Vector3(10.0, 10.0, 10.0), Vector3(5, 5, 5)), Wrench(Vector3(50.0, 50.0, 50.0), Vector3(25, 25, 25)), Wrench(Vector3(250.0, 250.0, 250.0), Vector3(100, 100, 100)), k_high ] for k in k_increase: raw_input("Press Enter to set bigger stiffness...") self.velma.updateTransformations() T_B_Wd = self.velma.T_B_W time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) print "moving the wrist to the current pose in " + str(time) + " s..." self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14))) rospy.sleep(time) print "done." print "setting stiffness to bigger value" self.velma.moveImpedance(k, 1.0) if self.velma.checkStopCondition(1.1): exit(0) print "done." if True: print "probing the door..." search_radius = 0.02 min_dist = 0.005 probed_points = [] contact_points_B = [] # move to the center point and push the lock x = 0 y = 0 T_O_Od = PyKDL.Frame(self.key_up_O*x + self.key_side_O*y) T_O_Od2 = PyKDL.Frame(self.key_up_O*x + self.key_side_O*y + self.key_axis_O*0.1) self.velma.updateTransformations() T_B_Wd = self.T_B_O_nearHole * T_O_Od * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) raw_input("Press ENTER to movie the wrist in " + str(time) + " s...") self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2) status, feedback = self.velma.waitForCartEnd() print "status", status if status.error_code != 0: print "unexpected contact", feedback exit(0) self.velma.updateTransformations() T_B_Wd = self.T_B_O_nearHole * T_O_Od2 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) raw_input("Press ENTER to movie the wrist in " + str(time) + " s...") self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol) status, feedback = self.velma.waitForCartEnd() print "status", status if status.error_code != 0: self.velma.updateTransformations() contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O contact_points_B.append(contact_B) m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None) print feedback else: print "no contact" exit(0) T_O_Od3 = PyKDL.Frame(self.key_axis_O * 5.0/k_high.force.z) self.velma.updateTransformations() T_B_Wref = self.velma.T_B_W T_B_Wd = T_B_Wref * self.velma.T_W_E * self.T_E_O * T_O_Od3 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) raw_input("Press ENTER to movie the wrist in " + str(time) + " s...") self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2) status, feedback = self.velma.waitForCartEnd() print "status", status if status.error_code != 0: print "unexpected high contact force", feedback exit(0) desired_push_dist = 5.0/k_high.force.z self.velma.updateTransformations() contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O contact_points_B.append(contact_B) # move along the spiral hole_found = False spiral = self.generateSpiral(search_radius, min_dist) for pt in spiral: pt_desired_O_in_ref = self.key_endpoint_O pt_contact_O_in_ref = self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() * T_B_Wref.Inverse() * contact_B key_end_depth = PyKDL.dot(pt_contact_O_in_ref-pt_desired_O_in_ref, self.key_axis_O) # print "key_end_depth", key_end_depth T_O_Od4 = PyKDL.Frame(self.key_up_O*pt[0] + self.key_side_O*pt[1] + self.key_axis_O * (5.0/k_high.force.z + key_end_depth)) pt_desired_B = T_B_Wref * self.velma.T_W_E * self.T_E_O * T_O_Od4 * self.key_endpoint_O m_id = self.pub_marker.publishSinglePointMarker(pt_desired_B, m_id, r=0, g=1, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None) T_B_Wd = T_B_Wref * self.velma.T_W_E * self.T_E_O * T_O_Od4 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) raw_input("Press ENTER to movie the wrist in " + str(time) + " s...") self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol) status, feedback = self.velma.waitForCartEnd() print "status", status self.velma.updateTransformations() contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O contact_points_B.append(contact_B) m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None) # check if the key is in the hole if status.error_code != 0: self.velma.updateTransformations() T_O_Od5 = PyKDL.Frame(self.key_axis_O * 5.0/k_high.force.z) T_B_Wd = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * T_O_Od5 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2) status, feedback = self.velma.waitForCartEnd() if status.error_code != 0: print "unexpected high contact force", feedback exit(0) self.velma.updateTransformations() T_O_Od6 = PyKDL.Frame(self.key_up_O*0.02 + self.key_axis_O * 5.0/k_high.force.z) T_B_Wd = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * T_O_Od6 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol) status, feedback = self.velma.waitForCartEnd() if status.error_code != 0: print "key is in a hole" hole_found = True break print "hole_found", hole_found T_O_Od3 = PyKDL.Frame(self.key_axis_O * 5.0/k_high.force.z) self.velma.updateTransformations() T_B_Wref = self.velma.T_B_W print "moving the wrist to the current pose to reduce tension..." T_B_Wd = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * T_O_Od3 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04) raw_input("Press ENTER to movie the wrist in " + str(time) + " s...") self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2) status, feedback = self.velma.waitForCartEnd() print "status", status if status.error_code != 0: print "unexpected high contact force", feedback exit(0) print "setting stiffness to bigger value" self.velma.moveImpedance(k_hole_in, 1.0) if self.velma.checkStopCondition(1.1): exit(0) print "done." self.velma.updateTransformations() # calculate the fixed lock frame T_B_L # T_B_L the same orientation as the gripper (z axis pointing towards the door) # and the origin at the key endpoint at the initial penetration T_B_L = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * PyKDL.Frame(self.key_endpoint_O) penetration_axis_L = PyKDL.Vector(0, 0, 1) # get current contact point contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O contact_L = T_B_L.Inverse() * contact_B contact_max_depth_L = PyKDL.dot(contact_L, penetration_axis_L) contact_depth_L = contact_max_depth_L prev_contact_depth_L = contact_depth_L deepest_contact_L = copy.deepcopy(contact_L) m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None) central_point_L = PyKDL.Vector() force_key_axis = 17.0 force_key_up = 10.0 force_key_side = 10.0 xi = 7 yi = 7 explore_mode = "get_new" while True: # desired position of the key end self.velma.updateTransformations() T_B_Ocurrent = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * PyKDL.Frame(self.key_axis_O * (force_key_axis/k_hole_in.force.x)) diff_B = PyKDL.diff(T_B_Ocurrent, self.T_B_O_nearHole) rot_diff_Ocurrent = PyKDL.Frame(T_B_Ocurrent.Inverse().M) * diff_B.rot spiral_hole = self.generateSpiral(0.04, 0.005) T_B_W_shake = [] for pt in spiral_hole: T_B_Od_shake = T_B_Ocurrent * PyKDL.Frame(PyKDL.Rotation.RotZ(rot_diff_Ocurrent.z()-6.0/180.0*math.pi), self.key_up_O * pt[0] + self.key_side_O * pt[1]) T_B_W_shake.append(T_B_Od_shake * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()) T_B_Od_shake = T_B_Ocurrent * PyKDL.Frame(PyKDL.Rotation.RotZ(rot_diff_Ocurrent.z()+6.0/180.0*math.pi), self.key_up_O * pt[0] + self.key_side_O * pt[1]) T_B_W_shake.append(T_B_Od_shake * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()) print "shaking..." counter = 0 for T_B_W in T_B_W_shake: counter += 1 time = self.velma.getMovementTime(T_B_W, max_v_l = 0.4, max_v_r = 0.4) # self.velma.moveWrist2(T_B_W * self.velma.T_W_T) # raw_input("Press ENTER to movie the wrist in " + str(time) + " s...") self.velma.moveWrist(T_B_W, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol_in) status1, feedback = self.velma.waitForCartEnd() print "status", status1 self.velma.updateTransformations() contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None) pt_desired_B = T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O m_id = self.pub_marker.publishSinglePointMarker(pt_desired_B, m_id, r=0, g=1, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None) contact_L = T_B_L.Inverse() * contact_B contact_depth_L = PyKDL.dot(contact_L, penetration_axis_L) if contact_depth_L > contact_max_depth_L+0.0001: deepest_contact_L = copy.deepcopy(contact_L) contact_max_depth_L = contact_depth_L print "contact_depth_L: %s"%(contact_depth_L) explore_mode = "push" break if status1.error_code != 0: break print "done." score = contact_depth_L - prev_contact_depth_L prev_contact_depth_L = contact_depth_L if status1.error_code != 0 or counter == len(T_B_W_shake): break print "moving the key to the current pose..." self.velma.updateTransformations() T_B_Wd = self.velma.T_B_W time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.1, max_v_r = 0.1) self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol_in2) status1, feedback = self.velma.waitForCartEnd() print "status", status1 if status1.error_code != 0: exit(0) print "moving the key to the current pose..." self.velma.updateTransformations() T_B_Ocurrent = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O T_B_Od = T_B_Ocurrent * PyKDL.Frame(self.key_axis_O * (-2.0/k_hole_in.force.x)) T_B_Wd = T_B_Od * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.1, max_v_r = 0.1) raw_input("Press ENTER to movie the wrist in " + str(time) + " s...") self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol_in) status1, feedback = self.velma.waitForCartEnd() print "status", status1 if status1.error_code != 0: exit(0) self.velma.moveHand([q_grasp[0]-q_pre, q_grasp[1]-q_pre, q_grasp[2]-q_pre, q_grasp[3]], hv, ht, 5000, True) rospy.sleep(2) print "setting stiffness to bigger value" self.velma.moveImpedance(k_high, 1.0) if self.velma.checkStopCondition(1.1): exit(0) print "done." T_B_Wd = T_B_Ocurrent * self.T_E_Orot.Inverse() * self.velma.T_W_E.Inverse() time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.02) raw_input("Press ENTER to movie the wrist in " + str(time) + " s...") self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2) status1, feedback = self.velma.waitForCartEnd() print "status", status1 if status1.error_code != 0: exit(0) self.velma.moveHand([q_grasp[0]+q_pre, q_grasp[1]+q_pre, q_grasp[2]+q_pre, q_grasp[3]], hv, ht, 5000, True) rospy.sleep(5) # m_id = self.pub_marker.publishSinglePointMarker(T_B_L * PyKDL.Vector(0.01*xi+ori_map_vis_offset[0], 0.01*yi+ori_map_vis_offset[1], score), m_id, r=0, g=0, b=1, a=0.5, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None) # m_id = self.pub_marker.publishSinglePointMarker(T_B_L * PyKDL.Vector(0.01*xi+ori_map_vis_offset[0], 0.01*yi+ori_map_vis_offset[1], ori_map[xi][yi][1]), m_id, r=0, g=1, b=0, a=0.5, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None) exit(0)
def getMovementTime(self, T_B_Wd, max_v_l = 0.1, max_v_r = 0.2): self.updateTransformations() twist = PyKDL.diff(self.T_B_W, T_B_Wd, 1.0) v_l = twist.vel.Norm() v_r = twist.rot.Norm() # print "v_l: %s v_r: %s"%(v_l, v_r) f_v_l = v_l/max_v_l f_v_r = v_r/max_v_r if f_v_l > f_v_r: duration = f_v_l else: duration = f_v_r if duration < 0.2: duration = 0.2 return duration