size of the vector doesn't match when using set_effort_joint

[zrwang]

Hi Anton, I was commanding the robot by joint effort via python wrapper today. However, I got the following error messages after sending psm1.set_effort_joint(np.array([0.1,0,0,0,0,0],dtype=np.float))

E- cmnThrow with std::exception (vctDynamicVectorLoopEngines: Sizes of vectors don't match)
-W mtsMailbox "Robot[PSM1]": ExecuteNext for command "SetEffortJoint" caught exception "vctDynamicVectorLoopEngines: Sizes of vectors don't match"
-W Class mtsIntuitiveResearchKitPSM: Task PSM1 caught run exception: vctDynamicVectorLoopEngines: Sizes of vectors don't match
E- cmnThrow with std::exception (vctDynamicVectorLoopEngines: Sizes of vectors don't match)
-W mtsMailbox "Robot[PSM1]": ExecuteNext for command "SetEffortJoint" caught exception "vctDynamicVectorLoopEngines: Sizes of vectors don't match"
-W Class mtsIntuitiveResearchKitPSM: Task PSM1 caught run exception: vctDynamicVectorLoopEngines: Sizes of vectors don't match
/home/zrwang/catkin_ws/src/cisst-saw/sawIntuitiveResearchKit/components/code/mtsIntuitiveResearchKitPSM.cpp: Assertion 'jointsKinematics.size() == 6' failed in: virtual void mtsIntuitiveResearchKitPSM::ToJointsPID(const vctDoubleVec&, vctDoubleVec&), line #235
E- File: mtsIntuitiveResearchKitPSM.cpp Line: 235 -  /home/zrwang/catkin_ws/src/cisst-saw/sawIntuitiveResearchKit/components/code/mtsIntuitiveResearchKitPSM.cpp: Assertion 'jointsKinematics.size() == 6' failed in: virtual void mtsIntuitiveResearchKitPSM::ToJointsPID(const vctDoubleVec&, vctDoubleVec&), line #235
Aborted (core dumped)

Could you help me look into this problem? I believe this is a problem of converting the command message into the low level PID control target. We are sending the effort message with length of 6, but we have 7 DoF on the PSM. Thank you so much for your help in advance.

Best regards, Zerui

[adeguet1]

Argh, we recently decided to maintain two sets of data members for joint level data in the arm class. One for the PID that matched the total level of joints controlled including kinematic chain and jaws. The other set is for the user API so the number of joints is based on the kinematic chain. Internally the class should agglomerate the 6 for kinematics + jaw effort to send to the PID.

[zrwang]

So what should I do in order to use the set_effort_joint command? Or it's supposed to be no problem?

[adeguet1]

What happens if you send 7 values?

[zrwang]

I did a hack by adding the following HACK line into arm.py, self.set_effort_joint.

    def set_effort_joint(self, effort):
        if (not self.__dvrk_set_state('DVRK_EFFORT_JOINT')):
            return False
        if ((not(type(effort) is numpy.ndarray))
            or (not(effort.dtype == numpy.float64))):
            print "effort must be an array of floats"
            return False
        if (not(effort.size == self.get_joint_number())):
            print "effort must be an array of size", self.get_joint_number()
            return False
        effort = numpy.append(effort, [0.])  # HACK
        joint_state = JointState()
        joint_state.effort[:] = effort.flat
        self.__set_effort_joint_pub.publish(joint_state)
        return True

The good news is it works. However, it cannot be turned off once it's in the effort joint mode. In addition, it's cannot be changed into another mode like cartesian position either. The error I got after clicking the Off button is as follows:

/home/zrwang/catkin_ws/src/cisst-saw/sawIntuitiveResearchKit/components/code/mtsIntuitiveResearchKitPSM.cpp: Assertion 'jointsKinematics.size() == 6' failed in: virtual void mtsIntuitiveResearchKitPSM::ToJointsPID(const vctDoubleVec&, vctDoubleVec&), line #235
E- File: mtsIntuitiveResearchKitPSM.cpp Line: 235 -  /home/zrwang/catkin_ws/src/cisst-saw/sawIntuitiveResearchKit/components/code/mtsIntuitiveResearchKitPSM.cpp: Assertion 'jointsKinematics.size() == 6' failed in: virtual void mtsIntuitiveResearchKitPSM::ToJointsPID(const vctDoubleVec&, vctDoubleVec&), line #235
Aborted (core dumped)

Is this easy to be solved? Let me know if I can help. Thanks!

[adnanmunawar]

This issue is not just limited to setting joint effort, its there is cartesian effort as well. Atleast on the current commit I am on (commit: b3cbd67). I am not sure if it has been fixed in later commits.

[adeguet1]

Adnan,

It seems that you’re running the last release, can you confirm this? Also, which are are you trying to control cartesian force mode? MTM, PSM or ECM?

For MTMs, the devel branch now has a couple of test programs, one in Matlab and one in Python (e.g. https://github.com/jhu-dvrk/dvrk-ros/blob/devel/dvrk_python/scripts/dvrk_mtm_test.py).

We’re still having some issue with the API on the PSMs. The current implementation treats the first 6 joints as kinematic joints and the last one is for the jaw. Ideally one should be able to drive these in two different modes (e.g. cartesian/joint position for first 6 dofs + effort jaws). My aim for the current release is to allow position/position and effort/effort. I’ll add more test programs to validate the implementation.

Anton

---

Anton Deguet www.lcsr.jhu.edu - www.malonecenter.jhu.edu

On Oct 27, 2017, at 17:27, Adnan Munawar notifications@github.com wrote:

This issue is not just limited to setting joint effort, its there is cartesian effort as well. Atleast on the current commit I am on (commit: b3cbd67 https://github.com/jhu-dvrk/dvrk-ros/commit/b3cbd675b056c424cfeff29ebaefe052675fe109). I am not sure if it has been fixed in later commits.

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/jhu-dvrk/dvrk-ros/issues/21#issuecomment-340091451, or mute the thread https://github.com/notifications/unsubscribe-auth/ABEX8wnixWQxm4K4fMU4oH0ZS3nM0s9Vks5swkqpgaJpZM4NrxGC.

[adnanmunawar]

That is correct, I am using the release version on the master branch. The issue is with the PSMs as you mentioned in your reply, but its with setting Wrench as well rather than directly setting joint torques.. Is there some document on the API regarding that? Is this issue related to the sawIntuitiveResearchKit package or the dvrk-ros package.

[adeguet1]

@zrwang , could you test the latest devel branch? I patched the code so one can set efforts on the joints used for the kinematic chain (either in joint or cartesian space) while applying a separate effort for the jaws. See Python example in commits baea3759275e498850f79b4c6ad049182425b1ac and 0794a4f9f743f140dad4bf2e133f2e99ca4aa84f If this solution works for you, feel free to close this ticket :-)

[zrwang]

@adeguet1 Thanks a lot for your efforts. I will test the code ASAP. I am closing this ticket for now.